public List <PointLatLngAlt> mineLocations()
{
List <PointLatLngAlt> lRet = new List <PointLatLngAlt>();
double bearingFromOriginToOtherRef = new PointLatLngAlt(OriginMarker.Position).GetBearing(OtherReferencePoint.Position);
double internalAngleOfTriangle = (getInternalAngleOfTriangle()) * FlightData.rad2deg;
if (double.IsNaN(bearingFromOriginToOtherRef) || double.IsNaN(internalAngleOfTriangle))
{
MessageBox.Show("invalid bearing ");
}
else
{
PointLatLngAlt lNewPoint = OriginMarker.Position;
lNewPoint = lNewPoint.newpos(bearingFromOriginToOtherRef - internalAngleOfTriangle, DistanceFromOriginToMarker * 1000);
lRet.Add(lNewPoint);
lNewPoint.groundTruth = true;
// lNewPoint.color = System.Drawing.Color.Yellow;
PointLatLngAlt lNewPoint2 = OriginMarker.Position;
lNewPoint2 = lNewPoint2.newpos(bearingFromOriginToOtherRef + internalAngleOfTriangle, DistanceFromOriginToMarker * 1000);
lRet.Add(lNewPoint2);
lNewPoint2.groundTruth = true;
// lNewPoint2.color = System.Drawing.Color.Yellow;
}
return(lRet);
}
public static List <PointLatLngAlt> GeneratePath(MAVState MAV)
{
List <PointLatLngAlt> result = new List <PointLatLngAlt>();
MAVLink.mavlink_mission_item_int_t?prevwp = null;
int a = -1;
foreach (var wp in MAV.wps.Values)
{
a++;
if (!prevwp.HasValue)
{
// change firstwp/aka home to valid alt
prevwp = new MAVLink.mavlink_mission_item_int_t?(new MAVLink.mavlink_mission_item_int_t()
{
x = wp.x, y = wp.y, z = 0
});
continue;
}
if (wp.command != (ushort)MAVLink.MAV_CMD.WAYPOINT && wp.command != (ushort)MAVLink.MAV_CMD.SPLINE_WAYPOINT)
{
continue;
}
var wp1 = new PointLatLngAlt(prevwp.Value);
var wp2 = new PointLatLngAlt(wp);
var bearing = wp1.GetBearing(wp2);
var distwps = wp1.GetDistance(wp2);
var startalt = wp1.Alt;
var endalt = wp2.Alt;
if (startalt == 0)
{
startalt = endalt;
}
if (distwps > 5000)
{
continue;
}
for (double d = 0.1; d < distwps; d += 0.1)
{
var pnt = wp1.newpos(bearing, d);
pnt.Alt = startalt + (d / distwps) * (endalt - startalt);
result.Add(pnt);
}
prevwp = wp;
}
return(result);
}
static PointLatLngAlt calcIntersection(PointLatLngAlt plla, PointLatLngAlt dest, int step = 100)
{
int distout = 10;
PointLatLngAlt newpos = PointLatLngAlt.Zero;
var dist = plla.GetDistance(dest);
var Y = plla.GetBearing(dest);
// 20 km
while (distout < (dist + 100))
{
// get a projected point to test intersection against - not using slope distance
PointLatLngAlt newposdist = plla.newpos(Y, distout);
newposdist.Alt = srtm.getAltitude(newposdist.Lat, newposdist.Lng).alt;
// get another point 'step' infront
PointLatLngAlt newposdist2 = plla.newpos(Y, distout + step);
newposdist2.Alt = srtm.getAltitude(newposdist2.Lat, newposdist2.Lng).alt;
// x is dist from plane, y is alt
var newpoint = FindLineIntersection(new PointF(0, (float)plla.Alt),
new PointF((float)dist, (float)dest.Alt),
new PointF((float)distout, (float)newposdist.Alt),
new PointF((float)distout + step, (float)newposdist2.Alt));
if (newpoint.X != 0)
{
newpos = plla.newpos(Y, newpoint.X);
newpos.Alt = newpoint.Y;
return(newpos);
}
distout += step;
}
//addtomap(newpos, distout.ToString());
return(plla);
}
public void UpdatePositions()
{
// add new point to trail
trail.Add(new PointLatLngAlt(GroundMasterDrone.MavState.cs.lat, GroundMasterDrone.MavState.cs.lng, GroundMasterDrone.MavState.cs.alt, ""));
while (trail.Count > 1000)
{
trail.RemoveAt(0);
}
// get current positions and velocitys
foreach (var drone in Drones)
{
if (drone.Location == null)
{
drone.Location = new PointLatLngAlt();
}
drone.Location.Lat = drone.MavState.cs.lat;
drone.Location.Lng = drone.MavState.cs.lng;
drone.Location.Alt = drone.MavState.cs.alt;
if (drone.Velocity == null)
{
drone.Velocity = new Vector3();
}
drone.Velocity.x = Math.Cos(drone.MavState.cs.groundcourse * deg2rad) * drone.MavState.cs.groundspeed;
drone.Velocity.y = Math.Sin(drone.MavState.cs.groundcourse * deg2rad) * drone.MavState.cs.groundspeed;
drone.Velocity.z = drone.MavState.cs.verticalspeed;
drone.TargetVelocity = GroundMasterDrone.Velocity;
}
var targetbearing = GroundMasterDrone.Heading;
//
if (GroundMasterDrone.MavState.cs.wp_dist < Seperation * 1.5)
{
var headingtowp = (int)GroundMasterDrone.MavState.cs.wpno;
var nextwp = headingtowp + 1;
try
{
PointLatLngAlt targetwp = new PointLatLngAlt(GroundMasterDrone.MavState.wps[headingtowp]);
//PointLatLngAlt targetwp = GroundMasterDrone.Location;
PointLatLngAlt nexttargetwp = new PointLatLngAlt(GroundMasterDrone.MavState.wps[nextwp]);
var bearing = targetwp.GetBearing(nexttargetwp);
// point on the wp line for target
var targetpnt = targetwp.newpos(bearing, Seperation);
targetbearing = GroundMasterDrone.Location.GetBearing(targetpnt);
if (Math.Abs(targetbearing - bearing) > 20)
{
//targetbearing = bearing;
}
AirMasterDrone.TargetVelocity.x = Math.Cos(targetbearing * deg2rad) *
GroundMasterDrone.MavState.cs.groundspeed;
AirMasterDrone.TargetVelocity.y = Math.Sin(targetbearing * deg2rad) *
GroundMasterDrone.MavState.cs.groundspeed;
}
catch
{
}
}
else
{
}
// calc airmaster position
AirMasterDrone.TargetLocation = GroundMasterDrone.Location.newpos(targetbearing, Seperation);
AirMasterDrone.TargetLocation.Alt = Altitude;
// send new position to airmaster
AirMasterDrone.SendPositionVelocity(AirMasterDrone.TargetLocation, AirMasterDrone.TargetVelocity * 0.6);
AirMasterDrone.MavState.GuidedMode.x = (float)AirMasterDrone.TargetLocation.Lat;
AirMasterDrone.MavState.GuidedMode.y = (float)AirMasterDrone.TargetLocation.Lng;
AirMasterDrone.MavState.GuidedMode.z = (float)AirMasterDrone.TargetLocation.Alt;
// get the path
List <PointLatLngAlt> newpositions = PlanMove();
List <PointLatLngAlt> newlist = new List <PointLatLngAlt>();
newlist.Add(GroundMasterDrone.Location);
newlist.AddRange(newpositions);
newpositions = newlist;
int a = 0;
// send position and velocity
foreach (var drone in Drones)
{
if (drone.MavState == airmaster)
{
continue;
}
if (drone.MavState == groundmaster)
{
continue;
}
if (a > (newpositions.Count - 1))
{
break;
}
newpositions[a].Alt += Altitude;
// spline control
drone.SendPositionVelocity(newpositions[a], drone.TargetVelocity / 2);
drone.MavState.GuidedMode.x = (float)newpositions[a].Lat;
drone.MavState.GuidedMode.y = (float)newpositions[a].Lng;
drone.MavState.GuidedMode.z = (float)newpositions[a].Alt;
// vel only
//drone.SendVelocity(drone.TargetVelocity);
a++;
}
}
protected override void OnPaint(System.Windows.Forms.PaintEventArgs e)
{
if (this.DesignMode)
{
return;
}
if (area.LocationMiddle.Lat == 0 && area.LocationMiddle.Lng == 0)
{
return;
}
try
{
base.OnPaint(e);
}
catch { return; }
double heightscale = (step / 90.0) * 1;
float yawradians = (float)(Math.PI * (rpy.Z * 1) / 180.0f);
//radians = 0;
float mouseY = (float)step / 10f;
cameraX = center.Lng; // -Math.Sin(yawradians) * mouseY; // multiplying by mouseY makes the
cameraY = center.Lat; // -Math.Cos(yawradians) * mouseY; // camera get closer/farther away with mouseY
cameraZ = (center.Alt < srtm.getAltitude(center.Lat, center.Lng).alt) ? (srtm.getAltitude(center.Lat, center.Lng).alt + 1) * heightscale : center.Alt * heightscale; // (srtm.getAltitude(lookZ, lookX, 20) + 100) * heighscale;
lookX = center.Lng + Math.Sin(yawradians) * mouseY;
lookY = center.Lat + Math.Cos(yawradians) * mouseY;
lookZ = cameraZ;
// cameraZ += 0.04;
GMapProvider type = GMap.NET.MapProviders.GoogleSatelliteMapProvider.Instance;
PureProjection prj = type.Projection;
int size = (int)(cameraZ * 150000);
// in front
PointLatLngAlt leftf = center.newpos(rpy.Z, size);
// behind
PointLatLngAlt rightf = center.newpos(rpy.Z, 50);
// left : 90 allows for 180 degree viewing angle
PointLatLngAlt left = center.newpos(rpy.Z - 90, size);
// right
PointLatLngAlt right = center.newpos(rpy.Z + 90, size);
double maxlat = Math.Max(left.Lat, Math.Max(right.Lat, Math.Max(leftf.Lat, rightf.Lat)));
double minlat = Math.Min(left.Lat, Math.Min(right.Lat, Math.Min(leftf.Lat, rightf.Lat)));
double maxlng = Math.Max(left.Lng, Math.Max(right.Lng, Math.Max(leftf.Lng, rightf.Lng)));
double minlng = Math.Min(left.Lng, Math.Min(right.Lng, Math.Min(leftf.Lng, rightf.Lng)));
// if (Math.Abs(area.Lat - maxlat) < 0.001)
{
}
// else
{
area = RectLatLng.FromLTRB(minlng, maxlat, maxlng, minlat);
}
GPoint topLeftPx = prj.FromLatLngToPixel(area.LocationTopLeft, zoom);
GPoint rightButtomPx = prj.FromLatLngToPixel(area.Bottom, area.Right, zoom);
GPoint pxDelta = new GPoint(rightButtomPx.X - topLeftPx.X, rightButtomPx.Y - topLeftPx.Y);
zoom = 21;
pxDelta.X = 9999;
int otherzoomlevel = 12;
// zoom based on pixel density
while (pxDelta.X > this.Width)
{
zoom--;
// current area
topLeftPx = prj.FromLatLngToPixel(area.LocationTopLeft, zoom);
rightButtomPx = prj.FromLatLngToPixel(area.Bottom, area.Right, zoom);
pxDelta = new GPoint(rightButtomPx.X - topLeftPx.X, rightButtomPx.Y - topLeftPx.Y);
}
otherzoomlevel = zoom - 4;
Console.WriteLine("zoom {0}", zoom);
// update once per seconds - we only read from disk, so need to let cahce settle
if (lastrefresh.AddSeconds(0.5) < DateTime.Now)
{
// get tiles - bg
core.Provider = type;
core.Position = LocationCenter;
// get zoom 10
core.Zoom = otherzoomlevel;
core.OnMapSizeChanged(this.Width, this.Height);
// get actual current zoom
core.Zoom = zoom;
core.OnMapSizeChanged(this.Width, this.Height);
lastrefresh = DateTime.Now;
}
else
{
//return;
}
float screenscale = this.Width / (float)this.Height;
MakeCurrent();
GL.MatrixMode(MatrixMode.Projection);
OpenTK.Matrix4 projection = OpenTK.Matrix4.CreatePerspectiveFieldOfView(120 * deg2rad, screenscale, 0.00001f, (float)step * 20000);
GL.LoadMatrix(ref projection);
Matrix4 modelview = Matrix4.LookAt((float)cameraX, (float)cameraY, (float)cameraZ, (float)lookX, (float)lookY, (float)lookZ, 0, 0, 1);
GL.MatrixMode(MatrixMode.Modelview);
// roll
modelview = Matrix4.Mult(modelview, Matrix4.CreateRotationZ(rpy.X * deg2rad));
// pitch
modelview = Matrix4.Mult(modelview, Matrix4.CreateRotationX((rpy.Y - 15) * -deg2rad));
GL.LoadMatrix(ref modelview);
GL.ClearColor(Color.CornflowerBlue);
GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit | ClearBufferMask.AccumBufferBit);
GL.LightModel(LightModelParameter.LightModelAmbient, new float[] { 1f, 1f, 1f, 1f });
// GL.Disable(EnableCap.Fog);
GL.Enable(EnableCap.Fog);
//GL.Enable(EnableCap.Lighting);
//GL.Enable(EnableCap.Light0);
GL.Fog(FogParameter.FogColor, new float[] { 100 / 255.0f, 149 / 255.0f, 237 / 255.0f, 1f });
//GL.Fog(FogParameter.FogDensity,0.1f);
GL.Fog(FogParameter.FogMode, (int)FogMode.Linear);
GL.Fog(FogParameter.FogStart, (float)step * 40);
GL.Fog(FogParameter.FogEnd, (float)(step * 50));
// GL.Enable(EnableCap.DepthTest);
GL.DepthFunc(DepthFunction.Always);
/*
* GL.Begin(BeginMode.LineStrip);
*
* GL.Color3(Color.White);
* GL.Vertex3(0, 0, 0);
*
* //GL.Color3(Color.Red);
* GL.Vertex3(area.Bottom, 0, area.Left);
*
* //GL.Color3(Color.Yellow);
* GL.Vertex3(lookX, lookY, lookZ);
*
* //GL.Color3(Color.Green);
* GL.Vertex3(cameraX, cameraY, cameraZ);
*
* GL.End();
*/
/*
* GL.PointSize(10);
* GL.Color4(Color.Yellow);
* GL.LineWidth(5);
*
*
* GL.Begin(PrimitiveType.LineStrip);
*
* //GL.Vertex3(new Vector3((float)center.Lng,(float)center.Lat,(float)(center.Alt * heightscale)));
* //GL.Vertex3(new Vector3(0, 0, 0));
* //GL.Vertex3(new Vector3((float)cameraX, (float)cameraY, (float)cameraZ));
* //GL.Color3(Color.Green);
* //GL.Vertex3(new Vector3((float)lookX, (float)lookY, (float)lookZ));
*
* GL.Vertex3(area.LocationTopLeft.Lng, area.LocationTopLeft.Lat, (float)cameraZ);
* GL.Vertex3(area.LocationTopLeft.Lng, area.LocationRightBottom.Lat, (float)cameraZ);
* GL.Vertex3(area.LocationRightBottom.Lng, area.LocationRightBottom.Lat, (float)cameraZ);
* GL.Vertex3(area.LocationRightBottom.Lng, area.LocationTopLeft.Lat, (float)cameraZ);
* GL.Vertex3(area.LocationTopLeft.Lng, area.LocationTopLeft.Lat, (float)cameraZ);
*
* GL.End();
*/
GL.Finish();
GL.PointSize((float)(step * 1));
GL.Color3(Color.Blue);
GL.Begin(PrimitiveType.Points);
GL.Vertex3(new Vector3((float)center.Lng, (float)center.Lat, (float)cameraZ));
GL.End();
//GL.ClampColor(ClampColorTarget.ClampReadColor, ClampColorMode.True);
/*
* GL.Enable(EnableCap.Blend);
* GL.DepthMask(false);
* GL.BlendFunc(BlendingFactorSrc.Zero, BlendingFactorDest.Src1Color);
* GL.DepthMask(true);
* GL.Disable(EnableCap.Blend);
*/
// textureid.Clear();
// get level 10 tiles
List <GPoint> tileArea1 = prj.GetAreaTileList(area, otherzoomlevel, 1);
// get type list at new zoom level
List <GPoint> tileArea2 = prj.GetAreaTileList(area, zoom, 2);
List <GPoint> tileArea = new List <GPoint>();
tileArea.AddRange(tileArea1);
tileArea.AddRange(tileArea2);
// get tiles & combine into one
foreach (var p in tileArea)
{
int localzoom = zoom;
core.tileDrawingListLock.AcquireReaderLock();
core.Matrix.EnterReadLock();
try
{
if (tileArea1.Contains(p))
{
localzoom = otherzoomlevel;
}
topLeftPx = prj.FromLatLngToPixel(area.LocationTopLeft, localzoom);
GMap.NET.Internals.Tile t = core.Matrix.GetTileWithNoLock(localzoom, p);
if (t.NotEmpty)
{
foreach (GMapImage img in t.Overlays)
{
if (!textureid.ContainsKey(p))
{
generateTexture(p, (Bitmap)img.Img);
}
}
}
else
{
}
}
finally
{
core.Matrix.LeaveReadLock();
core.tileDrawingListLock.ReleaseReaderLock();
}
//GMapImage tile = ((PureImageCache)Maps.MyImageCache.Instance).GetImageFromCache(type.DbId, p, zoom) as GMapImage;
//if (tile != null && !textureid.ContainsKey(p))
{
// generateTexture(p, (Bitmap)tile.Img);
}
if (textureid.ContainsKey(p))
{
int texture = textureid[p];
GL.Enable(EnableCap.Texture2D);
GL.BindTexture(TextureTarget.Texture2D, texture);
}
else
{
//Console.WriteLine("Missing tile");
continue;
}
long x = p.X * prj.TileSize.Width - topLeftPx.X;
long y = p.Y * prj.TileSize.Width - topLeftPx.Y;
long xr = p.X * prj.TileSize.Width;
long yr = p.Y * prj.TileSize.Width;
long x2 = (p.X + 1) * prj.TileSize.Width;
long y2 = (p.Y + 1) * prj.TileSize.Width;
GL.LineWidth(0);
GL.Color3(Color.White);
// generate terrain
GL.Begin(PrimitiveType.TriangleStrip);
var latlng = prj.FromPixelToLatLng(xr, yr, localzoom);
double heightl = srtm.getAltitude(latlng.Lat, latlng.Lng).alt;
if (localzoom == 10)
{
heightl = 0;
}
//xr - topLeftPx.X, yr - topLeftPx.Y
GL.TexCoord2(0, 0);
GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale);
// next down
latlng = prj.FromPixelToLatLng(xr, y2, localzoom);
heightl = srtm.getAltitude(latlng.Lat, latlng.Lng).alt;
if (localzoom == 10)
{
heightl = 0;
}
GL.TexCoord2(0, 1);
GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale);
// next right
latlng = prj.FromPixelToLatLng(x2, yr, localzoom);
heightl = srtm.getAltitude(latlng.Lat, latlng.Lng).alt;
if (localzoom == 10)
{
heightl = 0;
}
GL.TexCoord2(1, 0);
GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale);
// next right down
latlng = prj.FromPixelToLatLng(x2, y2, localzoom);
heightl = srtm.getAltitude(latlng.Lat, latlng.Lng).alt;
if (localzoom == 10)
{
heightl = 0;
}
GL.TexCoord2(1, 1);
GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale);
GL.End();
}
GL.Flush();
try
{
this.SwapBuffers();
Context.MakeCurrent(null);
}
catch { }
//this.Invalidate();
return;
}
private void map_MouseMove(object sender, MouseEventArgs e)
{
var mousecurrent = map.FromLocalToLatLng(e.X, e.Y);
if (mousedown)
{
mousedragging = true;
if (currentmode == mode.panmode)
{
if (e.Button == MouseButtons.Left)
{
double latdif = mousestart.Lat - mousecurrent.Lat;
double lngdif = mousestart.Lng - mousecurrent.Lng;
try
{
map.Position = new PointLatLng(map.Position.Lat + latdif, map.Position.Lng + lngdif);
}
catch { }
}
}
else if (currentmode == mode.drawbox)
{
if (e.Button == MouseButtons.Left)
{
var rect = RectangleF.FromLTRB((float)mousestart.Lng, (float)mousestart.Lat, (float)mousecurrent.Lng, (float)mousecurrent.Lat);
list.Clear();
// tl
list.Add(mousestart);
// tr
list.Add(new PointLatLng(rect.Top, rect.Right));
// br
list.Add(mousecurrent);
// bl
list.Add(new PointLatLng(rect.Bottom, rect.Left));
if (boxpoly != null)
{
layerpolygons.Polygons.Remove(boxpoly);
}
boxpoly = null;
boxpoly = new GMapPolygon(list, "boxpoly");
boxpoly.IsHitTestVisible = true;
boxpoly.Stroke = new Pen(Color.Red, 2);
boxpoly.Fill = Brushes.Transparent;
layerpolygons.Polygons.Add(boxpoly);
map.Invalidate();
}
}
else if (currentmode == mode.movebox)
{
//if (mouseinsidepoly)
{
double latdif = mousestart.Lat - mousecurrent.Lat;
double lngdif = mousestart.Lng - mousecurrent.Lng;
for (int a = 0; a < boxpoly.Points.Count; a++)
{
boxpoly.Points[a] = new PointLatLng(boxpoly.Points[a].Lat - latdif, boxpoly.Points[a].Lng - lngdif);
}
UpdateListFromBox();
map.UpdatePolygonLocalPosition(boxpoly);
map.Invalidate();
mousestart = mousecurrent;
}
}
else if (currentmode == mode.editbox)
{
double latdif = mousestart.Lat - mousecurrent.Lat;
double lngdif = mousestart.Lng - mousecurrent.Lng;
// 2 point the create the lowest crosstrack distance
// extend at 90 degrees to the bearing of the points based on mouse position
PointLatLngAlt p0;
PointLatLngAlt p1;
PointLatLngAlt bestp0 = PointLatLngAlt.Zero;
PointLatLngAlt bestp1 = PointLatLngAlt.Zero;
double bestcrosstrack = 9999999;
double R = 6371000;
for (int a = 0; a < boxpoly.Points.Count; a++)
{
p0 = boxpoly.Points[a];
p1 = boxpoly.Points[(a + 1) % (boxpoly.Points.Count)];
var distp0p1 = p0.GetDistance(mousecurrent);
var bearingp0curr = p0.GetBearing(mousecurrent);
var bearringp0p1 = p0.GetBearing(p1);
var ct = Math.Asin(Math.Sin(distp0p1 / R) * Math.Sin((bearingp0curr - bearringp0p1) * deg2rad)) * R;
if (Math.Abs(ct) < Math.Abs(bestcrosstrack))
{
bestp0 = p0;
bestp1 = p1;
bestcrosstrack = ct;
}
}
var bearing = bestp0.GetBearing(bestp1);
layerpolygons.Markers.Clear();
layerpolygons.Markers.Add(new GMarkerGoogle(bestp0, GMarkerGoogleType.blue));
layerpolygons.Markers.Add(new GMarkerGoogle(bestp1, GMarkerGoogleType.blue));
bearing = ((PointLatLngAlt)mousestart).GetBearing(mousecurrent);
var newposp0 = bestp0.newpos(bearing, Math.Abs(bestcrosstrack));
var newposp1 = bestp1.newpos(bearing, Math.Abs(bestcrosstrack));
boxpoly.Points[boxpoly.Points.IndexOf(bestp0)] = newposp0;
boxpoly.Points[boxpoly.Points.IndexOf(bestp1)] = newposp1;
UpdateListFromBox();
map.UpdatePolygonLocalPosition(boxpoly);
map.Invalidate();
mousestart = mousecurrent;
}
}
mousedragging = false;
}
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.ToArray())
{
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 = ((Vector3)offsets[mav]).x;
var y = ((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);
// convert back to wgs84
IMathTransform inversedTransform = trans.MathTransform.Inverse();
double[] point = inversedTransform.Transform(p1);
target.Lat = point[1];
target.Lng = point[0];
target.Alt += ((Vector3)offsets[mav]).z;
if (mav.cs.firmware == Firmwares.ArduPlane)
{
// get distance from target position
var dist = target.GetDistance(mav.cs.Location);
// get bearing to target
var targyaw = mav.cs.Location.GetBearing(target);
var targettrailer = target.newpos(Leader.cs.yaw, Math.Abs(dist) * -0.25);
var targetleader = target.newpos(Leader.cs.yaw, 10 + dist);
var yawerror = wrap_180(targyaw - mav.cs.yaw);
var mavleadererror = wrap_180(Leader.cs.yaw - mav.cs.yaw);
if (dist < 100)
{
targyaw = mav.cs.Location.GetBearing(targetleader);
yawerror = wrap_180(targyaw - mav.cs.yaw);
var targBearing = mav.cs.Location.GetBearing(target);
// check the bearing for the leader and target are within 45 degrees.
if (Math.Abs(wrap_180(targBearing - targyaw)) > 45)
{
dist *= -1;
}
}
else
{
targyaw = mav.cs.Location.GetBearing(targettrailer);
yawerror = wrap_180(targyaw - mav.cs.yaw);
}
// display update
mav.GuidedMode.x = (float)target.Lat;
mav.GuidedMode.y = (float)target.Lng;
mav.GuidedMode.z = (float)target.Alt;
MAVLink.mavlink_set_attitude_target_t att_target = new MAVLink.mavlink_set_attitude_target_t();
att_target.target_system = mav.sysid;
att_target.target_component = mav.compid;
att_target.type_mask = 0xff;
Tuple <PID, PID, PID, PID> pid;
if (pids.ContainsKey(mav))
{
pid = pids[mav];
}
else
{
pid = new Tuple <PID, PID, PID, PID>(
new PID(1f, .03f, 0.02f, 10, 20, 0.1f, 0),
new PID(1f, .03f, 0.02f, 10, 20, 0.1f, 0),
new PID(1, 0, 0.00f, 15, 20, 0.1f, 0),
new PID(0.01f, 0.001f, 0, 0.5f, 20, 0.1f, 0));
pids.Add(mav, pid);
}
var rollp = pid.Item1;
var pitchp = pid.Item2;
var yawp = pid.Item3;
var thrustp = pid.Item4;
var newroll = 0d;
var newpitch = 0d;
if (true)
{
var altdelta = target.Alt - mav.cs.alt;
newpitch = altdelta;
att_target.type_mask -= 0b00000010;
pitchp.set_input_filter_all((float)altdelta);
newpitch = pitchp.get_pid();
}
if (true)
{
var leaderturnrad = Leader.cs.radius;
var mavturnradius = leaderturnrad - x;
{
var distToTarget = mav.cs.Location.GetDistance(target);
var bearingToTarget = mav.cs.Location.GetBearing(target);
// bearing stability
if (distToTarget < 30)
{
bearingToTarget = mav.cs.Location.GetBearing(targetleader);
}
// fly in from behind
if (distToTarget > 100)
{
bearingToTarget = mav.cs.Location.GetBearing(targettrailer);
}
var bearingDelta = wrap_180(bearingToTarget - mav.cs.yaw);
var tangent90 = bearingDelta > 0 ? 90 : -90;
newroll = 0;
// if the delta is > 90 then we are facing the wrong direction
if (Math.Abs(bearingDelta) < 85)
{
var insideAngle = Math.Abs(tangent90 - bearingDelta);
var angleCenter = 180 - insideAngle * 2;
// sine rule
var sine1 = Math.Max(distToTarget, 40) /
Math.Sin(angleCenter * MathHelper.deg2rad);
var radius = sine1 * Math.Sin(insideAngle * MathHelper.deg2rad);
// average calced + leader offset turnradius - acts as a FF
radius = (Math.Abs(radius) + Math.Abs(mavturnradius)) / 2;
var angleBank = ((mav.cs.groundspeed * mav.cs.groundspeed) / radius) / 9.8;
angleBank *= MathHelper.rad2deg;
if (bearingDelta > 0)
{
newroll = Math.Abs(angleBank);
}
else
{
newroll = -Math.Abs(angleBank);
}
}
newroll += MathHelper.constrain(bearingDelta, -20, 20);
}
// tr = gs2 / (9.8 * x)
// (9.8 * x) * tr = gs2
// 9.8 * x = gs2 / tr
// (gs2/tr)/9.8 = x
var angle = ((mav.cs.groundspeed * mav.cs.groundspeed) / mavturnradius) / 9.8;
//newroll = angle * MathHelper.rad2deg;
// 1 degree of roll for ever 1 degree of yaw error
//newroll += MathHelper.constrain(yawerror, -20, 20);
//rollp.set_input_filter_all((float)yawdelta);
}
// do speed
if (true)
{
//att_target.thrust = (float) MathHelper.mapConstrained(dist, 0, 40, 0, 1);
att_target.type_mask -= 0b01000000;
// in m out 0-1
thrustp.set_input_filter_all((float)dist);
// prevent buildup prior to being close
if (dist > 40)
{
thrustp.reset_I();
}
// 0.1 demand + pid results
att_target.thrust = (float)MathHelper.constrain(thrustp.get_pid(), 0.1, 1);
}
Quaternion q = new Quaternion();
q.from_vector312(newroll * MathHelper.deg2rad, newpitch * MathHelper.deg2rad, yawerror * MathHelper.deg2rad);
att_target.q = new float[4];
att_target.q[0] = (float)q.q1;
att_target.q[1] = (float)q.q2;
att_target.q[2] = (float)q.q3;
att_target.q[3] = (float)q.q4;
//0b0= rpy
att_target.type_mask -= 0b10000101;
//att_target.type_mask -= 0b10000100;
Console.WriteLine("sysid {0} - {1} dist {2} r {3} p {4} y {5}", mav.sysid,
att_target.thrust, dist, newroll, newpitch, (targyaw - mav.cs.yaw));
/* Console.WriteLine("rpyt {0} {1} {2} {3} I {4} {5} {6} {7}",
* rollp.get_pid(), pitchp.get_pid(), yawp.get_pid(), thrustp.get_pid(),
* rollp.get_i(), pitchp.get_i(), yawp.get_i(), thrustp.get_i());
*/
port.sendPacket(att_target, mav.sysid, mav.compid);
}
else
{
Vector3 vel = new Vector3(Leader.cs.vx, Leader.cs.vy, Leader.cs.vz);
// do pos/vel
port.setPositionTargetGlobalInt(mav.sysid, mav.compid, true,
true, false, false,
MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT, target.Lat, target.Lng, target.Alt, vel.x,
vel.y, vel.z, 0, 0);
// 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++;
}
}
}
//http://www.chrobotics.com/library/understanding-euler-angles
// x = north / roll
// y = pitch / east
// z = yaw / down
public static List <PointLatLngAlt> calc(PointLatLngAlt plla, double R, double P, double Y, double hfov, double vfov)
{
// alt should be asl
// P pitch where the center os pointing ie -80
// R roll
// if roll and pitch is 0, use the quick method.
if (R == 0 && P == 0)
{
// calc fov in m on the ground at 0 alt
var fovh = Math.Tan(hfov / 2.0 * MathHelper.deg2rad) * plla.Alt;
var fovv = Math.Tan(vfov / 2.0 * MathHelper.deg2rad) * plla.Alt;
var fovd = Math.Sqrt(fovh * fovh + fovv * fovv);
// where we put our footprint
var ans2 = new List <PointLatLngAlt>();
// calc bearing from center to corner
var bearing1 = Math.Atan2(fovh, fovv) * MathHelper.rad2deg;
// calc first corner point
var newpos1 = plla.newpos(bearing1 + Y, Math.Sqrt(fovh * fovh + fovv * fovv));
// set alt to 0, as we used the hypot for distance and fov is based on 0 alt
newpos1.Alt = 0;
// calc intersection from center to new point and return ground hit point
var cen1 = calcIntersection(plla, newpos1);
// add to our footprint poly
ans2.Add(cen1);
addtomap(cen1, "cen1");
//repeat
newpos1 = plla.newpos(Y - bearing1, Math.Sqrt(fovh * fovh + fovv * fovv));
newpos1.Alt = 0;
cen1 = calcIntersection(plla, newpos1);
ans2.Add(cen1);
addtomap(cen1, "cen2");
newpos1 = plla.newpos(bearing1 + Y - 180, Math.Sqrt(fovh * fovh + fovv * fovv));
newpos1.Alt = 0;
cen1 = calcIntersection(plla, newpos1);
ans2.Add(cen1);
addtomap(cen1, "cen3");
newpos1 = plla.newpos(Y - bearing1 - 180, Math.Sqrt(fovh * fovh + fovv * fovv));
newpos1.Alt = 0;
cen1 = calcIntersection(plla, newpos1);
ans2.Add(cen1);
addtomap(cen1, "cen4");
addtomap(plla, "plane");
return(ans2);
}
GMapPolygon poly = new GMapPolygon(new List <PointLatLng>(), "rect");
double frontangle = (P * 0) + vfov / 2;
double backangle = (P * 0) - vfov / 2;
double leftangle = (R * 0) + hfov / 2;
double rightangle = (R * 0) - hfov / 2;
addtomap(plla, "plane");
Matrix3 dcm = new Matrix3();
dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad);
dcm.normalize();
Vector3 center1 = new Vector3(0, 0, 10000);
var test = dcm * center1;
var bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg;
var newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
var cen = calcIntersection(plla, newpos2);
var dist = plla.GetDistance(cen);
addtomap(cen, "center " + dist.ToString("0"));
//
dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad);
dcm.rotate(new Vector3(rightangle * MathHelper.deg2rad, 0, 0));
dcm.normalize();
dcm.rotate(new Vector3(0, frontangle * MathHelper.deg2rad, 0));
dcm.normalize();
/*
* var mx = new Matrix3();
* var my = new Matrix3();
* var mz = new Matrix3();
*
* mx.from_euler((rightangle + R) * MathHelper.deg2rad, 0, 0);
* my.from_euler(0, (frontangle + P) * MathHelper.deg2rad, 0);
* mz.from_euler(0, 0, Y * MathHelper.deg2rad);
*
* printdcm(mx);
* printdcm(my);
* printdcm(mz);
* printdcm(my * mx);
* printdcm(mz * my * mx);
*
* test = mz * my * mx * center1;
*/
test = dcm * center1;
bearing2 = (Math.Atan2(test.y, test.x) * MathHelper.rad2deg);
newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
//addtomap(newpos2, "tr2");
var groundpointtr = calcIntersection(plla, newpos2);
poly.Points.Add(groundpointtr);
addtomap(groundpointtr, "tr");
//
dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad);
dcm.rotate(new Vector3(leftangle * MathHelper.deg2rad, 0, 0));
dcm.normalize();
dcm.rotate(new Vector3(0, frontangle * MathHelper.deg2rad, 0));
dcm.normalize();
test = dcm * center1;
bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg;
newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
var groundpointtl = calcIntersection(plla, newpos2);
poly.Points.Add(groundpointtl);
addtomap(groundpointtl, "tl");
//
dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad);
dcm.rotate(new Vector3(leftangle * MathHelper.deg2rad, 0, 0));
dcm.normalize();
dcm.rotate(new Vector3(0, backangle * MathHelper.deg2rad, 0));
dcm.normalize();
test = dcm * center1;
bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg;
newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
var groundpointbl = calcIntersection(plla, newpos2);
poly.Points.Add(groundpointbl);
addtomap(groundpointbl, "bl");
//
dcm.from_euler(R * MathHelper.deg2rad, P * MathHelper.deg2rad, Y * MathHelper.deg2rad);
dcm.rotate(new Vector3(rightangle * MathHelper.deg2rad, 0, 0));
dcm.normalize();
dcm.rotate(new Vector3(0, backangle * MathHelper.deg2rad, 0));
dcm.normalize();
test = dcm * center1;
bearing2 = Math.Atan2(test.y, test.x) * MathHelper.rad2deg;
newpos2 = plla.newpos(bearing2, Math.Sqrt(test.x * test.x + test.y * test.y));
newpos2.Alt -= test.z;
var groundpointbr = calcIntersection(plla, newpos2);
poly.Points.Add(groundpointbr);
addtomap(groundpointbr, "br");
//
polygons.Polygons.Clear();
polygons.Polygons.Add(poly);
var ans = new List <PointLatLngAlt>();
ans.Add(groundpointtl);
ans.Add(groundpointtr);
ans.Add(groundpointbr);
ans.Add(groundpointbl);
return(ans);
}
protected override void OnPaint(System.Windows.Forms.PaintEventArgs e)
{
if (this.DesignMode)
{
return;
}
if (area.LocationMiddle.Lat == 0 && area.LocationMiddle.Lng == 0)
{
return;
}
try
{
base.OnPaint(e);
}
catch { return; }
double heightscale = (step / 90.0) * 3;
float yawradians = (float)(Math.PI * (rpy.Z * 1) / 180.0f);
//radians = 0;
float mouseY = (float)(0.0025);
cameraX = area.LocationMiddle.Lng; // -Math.Sin(yawradians) * mouseY; // multiplying by mouseY makes the
cameraY = area.LocationMiddle.Lat; // -Math.Cos(yawradians) * mouseY; // camera get closer/farther away with mouseY
cameraZ = (LocationCenter.Alt < srtm.getAltitude(center.Lat, center.Lng)) ? (srtm.getAltitude(center.Lat, center.Lng) + 1) * heightscale : LocationCenter.Alt * heightscale; // (srtm.getAltitude(lookZ, lookX, 20) + 100) * heighscale;
lookX = area.LocationMiddle.Lng + Math.Sin(yawradians) * mouseY;
lookY = area.LocationMiddle.Lat + Math.Cos(yawradians) * mouseY;
lookZ = cameraZ;
// cameraZ += 0.04;
GMapProvider type = GMap.NET.MapProviders.GoogleSatelliteMapProvider.Instance;
PureProjection prj = type.Projection;
PointLatLngAlt leftf = center.newpos(rpy.Z, 500);
PointLatLngAlt rightf = center.newpos(rpy.Z, 10);
PointLatLngAlt left = center.newpos(rpy.Z - 90, 500);
PointLatLngAlt right = center.newpos(rpy.Z + 90, 500);
double maxlat = Math.Max(left.Lat, Math.Max(right.Lat, Math.Max(leftf.Lat, rightf.Lat)));
double minlat = Math.Min(left.Lat, Math.Min(right.Lat, Math.Min(leftf.Lat, rightf.Lat)));
double maxlng = Math.Max(left.Lng, Math.Max(right.Lng, Math.Max(leftf.Lng, rightf.Lng)));
double minlng = Math.Min(left.Lng, Math.Min(right.Lng, Math.Min(leftf.Lng, rightf.Lng)));
area = RectLatLng.FromLTRB(minlng, maxlat, maxlng, minlat);
GPoint topLeftPx = prj.FromLatLngToPixel(area.LocationTopLeft, zoom);
GPoint rightButtomPx = prj.FromLatLngToPixel(area.Bottom, area.Right, zoom);
GPoint pxDelta = new GPoint(rightButtomPx.X - topLeftPx.X, rightButtomPx.Y - topLeftPx.Y);
zoom = 17;
// zoom based on pixel density
while (pxDelta.X > 2000)
{
zoom--;
// current area
topLeftPx = prj.FromLatLngToPixel(area.LocationTopLeft, zoom);
rightButtomPx = prj.FromLatLngToPixel(area.Bottom, area.Right, zoom);
pxDelta = new GPoint(rightButtomPx.X - topLeftPx.X, rightButtomPx.Y - topLeftPx.Y);
}
// update once per seconds - we only read from disk, so need to let cahce settle
if (lastrefresh.AddSeconds(.6) < DateTime.Now)
{
// get tiles - bg
core.Provider = type;
core.Position = LocationCenter;
core.Zoom = zoom;
core.OnMapSizeChanged(this.Width, this.Height);
lastrefresh = DateTime.Now;
}
else
{
//return;
}
MakeCurrent();
GL.MatrixMode(MatrixMode.Projection);
OpenTK.Matrix4 projection = OpenTK.Matrix4.CreatePerspectiveFieldOfView(130 * deg2rad, 1f, 0.00001f, (float)step * 100);
GL.LoadMatrix(ref projection);
Matrix4 modelview = Matrix4.LookAt((float)cameraX, (float)cameraY, (float)cameraZ, (float)lookX, (float)lookY, (float)lookZ, 0, 0, 1);
GL.MatrixMode(MatrixMode.Modelview);
// roll
modelview = Matrix4.Mult(modelview, Matrix4.CreateRotationZ(rpy.X * deg2rad));
// pitch
modelview = Matrix4.Mult(modelview, Matrix4.CreateRotationX((rpy.Y - 10) * -deg2rad));
GL.LoadMatrix(ref modelview);
GL.ClearColor(Color.LightBlue);
GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit);
GL.LightModel(LightModelParameter.LightModelAmbient, new float[] { 1f, 1f, 1f, 1f });
GL.Enable(EnableCap.Fog);
// GL.Enable(EnableCap.Lighting);
// GL.Enable(EnableCap.Light0);
GL.Fog(FogParameter.FogColor, new float[] { 0.5f, 0.5f, 0.5f, 1f });
//GL.Fog(FogParameter.FogDensity,0.1f);
GL.Fog(FogParameter.FogMode, (int)FogMode.Linear);
GL.Fog(FogParameter.FogStart, (float)step * 1);
GL.Fog(FogParameter.FogEnd, (float)(step * 7));
/*
* GL.Begin(BeginMode.LineStrip);
*
* GL.Color3(Color.White);
* GL.Vertex3(0, 0, 0);
*
* GL.Vertex3(area.Bottom, 0, area.Left);
*
* GL.Vertex3(lookX, lookY, lookZ);
*
* //GL.Vertex3(cameraX, cameraY, cameraZ);
*
* GL.End();
*/
GL.Begin(PrimitiveType.LineStrip);
GL.PointSize(200);
GL.Color3(Color.Red);
//GL.Vertex3(new Vector3((float)center.Lng,(float)center.Lat,(float)(center.Alt * heightscale)));
//GL.Vertex3(new Vector3(0, 0, 0));
//GL.Vertex3(new Vector3((float)cameraX, (float)cameraY, (float)cameraZ));
//GL.Color3(Color.Green);
//GL.Vertex3(new Vector3((float)lookX, (float)lookY, (float)lookZ));
GL.Vertex3(area.LocationTopLeft.Lng, area.LocationTopLeft.Lat, 0);
GL.Vertex3(area.LocationTopLeft.Lng, area.LocationRightBottom.Lat, 0);
GL.Vertex3(area.LocationRightBottom.Lng, area.LocationRightBottom.Lat, 0);
GL.Vertex3(area.LocationRightBottom.Lng, area.LocationTopLeft.Lat, 0);
GL.Vertex3(area.LocationTopLeft.Lng, area.LocationTopLeft.Lat, 0);
GL.End();
GL.Begin(PrimitiveType.LineStrip);
GL.PointSize(200);
GL.Color3(Color.Red);
GL.Vertex3(new Vector3((float)center.Lng, (float)center.Lat, 0));
GL.Vertex3(new Vector3((float)leftf.Lng, (float)leftf.Lat, 0));
GL.End();
GL.Begin(PrimitiveType.Points);
GL.PointSize(100);
GL.Color3(Color.Blue);
GL.Vertex3(new Vector3((float)center.Lng, (float)center.Lat, 0));
GL.End();
//textureid.Clear();
// get type list at new zoom level
List <GPoint> tileArea = prj.GetAreaTileList(area, zoom, 0);
// get tiles & combine into one
foreach (var p in tileArea)
{
core.tileDrawingListLock.AcquireReaderLock();
core.Matrix.EnterReadLock();
try
{
GMap.NET.Internals.Tile t = core.Matrix.GetTileWithNoLock(core.Zoom, p);
if (t.NotEmpty)
{
foreach (GMapImage img in t.Overlays)
{
if (!textureid.ContainsKey(p))
{
generateTexture(p, (Bitmap)img.Img);
}
}
}
else
{
}
}
finally
{
core.Matrix.LeaveReadLock();
core.tileDrawingListLock.ReleaseReaderLock();
}
//GMapImage tile = ((PureImageCache)Maps.MyImageCache.Instance).GetImageFromCache(type.DbId, p, zoom) as GMapImage;
//if (tile != null && !textureid.ContainsKey(p))
{
// generateTexture(p, (Bitmap)tile.Img);
}
if (textureid.ContainsKey(p))
{
int texture = textureid[p];
GL.Enable(EnableCap.Texture2D);
GL.BindTexture(TextureTarget.Texture2D, texture);
}
else
{
GL.Disable(EnableCap.Texture2D);
}
long x = p.X * prj.TileSize.Width - topLeftPx.X;
long y = p.Y * prj.TileSize.Width - topLeftPx.Y;
long xr = p.X * prj.TileSize.Width;
long yr = p.Y * prj.TileSize.Width;
long x2 = (p.X + 1) * prj.TileSize.Width;
long y2 = (p.Y + 1) * prj.TileSize.Width;
// generate terrain
GL.Begin(PrimitiveType.TriangleStrip);
var latlng = prj.FromPixelToLatLng(xr, yr, zoom);
double heightl = srtm.getAltitude(latlng.Lat, latlng.Lng);
GL.Color3(Color.White);
//xr - topLeftPx.X, yr - topLeftPx.Y
GL.TexCoord2(0, 0);
GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale);
// next down
latlng = prj.FromPixelToLatLng(xr, y2, zoom);
heightl = srtm.getAltitude(latlng.Lat, latlng.Lng);
GL.TexCoord2(0, .99);
GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale);
// next right
latlng = prj.FromPixelToLatLng(x2, yr, zoom);
heightl = srtm.getAltitude(latlng.Lat, latlng.Lng);
GL.TexCoord2(.99, 0);
GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale);
// next right down
latlng = prj.FromPixelToLatLng(x2, y2, zoom);
heightl = srtm.getAltitude(latlng.Lat, latlng.Lng);
GL.TexCoord2(.99, .99);
GL.Vertex3(latlng.Lng, latlng.Lat, heightl * heightscale);
GL.End();
}
GL.Enable(EnableCap.Blend);
GL.DepthMask(false);
GL.BlendFunc(BlendingFactorSrc.SrcAlpha, BlendingFactorDest.One);
GL.DepthMask(true);
GL.Disable(EnableCap.Blend);
GL.Flush();
try
{
this.SwapBuffers();
Context.MakeCurrent(null);
}
catch { }
return;
}
