/// <summary>
/// this constructor is used only for reading mission files
/// </summary>
/// <param name="lw"></param>
internal LocationWp(Locationwp lw)
{
waypointState = WaypointState.None;
number = lw.number;
id = (MAV_CMD)Enum.Parse(typeof(MAV_CMD), lw.id.ToString()); // command id
isHome = lw.ishome != 0;
coordinateFrameOption = lw.options == 1 ? CoordinateFrameOption.MAV_FRAME_GLOBAL_RELATIVE_ALT : CoordinateFrameOption.MAV_FRAME_GLOBAL;
// alt is in meters, can be above the ground (AGL) or above mean sea level (MSL), depending on coordinateFrameOption
geoPosition = new GeoPosition(lw.lng, lw.lat, lw.alt);
// p1-p4 are just float point numbers that can be added to waypoints and be interpreted by behaviors. We pass them all directly.
p1 = lw.p1;
p2 = lw.p2;
p3 = lw.p3;
p4 = lw.p4;
}
public GeoPosition subtract(GeoPosition a, bool spans180)
{
double x = a.X;
double dx = m_X - x;
if (spans180)
{ // dx < 360.0 && Math.Abs(dx) > 180.0) {
if (x > 90.0 && m_X < -90)
{
x -= 360.0;
}
else if (m_X > 90.0 && x < -90)
{
x += 360.0;
}
dx = m_X - x;
}
double dy = m_Y - a.Y;
double dz = m_H - a.H;
return(new GeoPosition(dx, dy, dz));
}
public DetectedObstacle(GeoPosition pos)
: base(pos)
{
SetObstacle();
}
/// <summary>
/// recalculates relPosition
/// </summary>
/// <param name="myPos">usually robot position</param>
/// <param name="myDir">usually robot direction</param>
public void updateRelPosition(GeoPosition myPos, Direction myDir)
{
this.relPosition = new RelPosition(this.directionTo(myPos, myDir), this.distanceTo(myPos));
}
} // we should only set it via updateRelPosition()
/// <summary>
/// recalculates relPosition
/// </summary>
/// <param name="myPos">usually robot position</param>
/// <param name="myDir">usually robot direction</param>
public void updateRelPosition(GeoPosition myPos, Direction myDir)
{
this.relPosition = new RelPosition(this.directionTo(myPos, myDir), this.distanceTo(myPos));
}
public DetectedObstacle(GeoPosition pos)
: base(pos)
{
objectType = DetectedObjectType.Obstacle;
}
public GeoPosition(GeoPosition loc)
{
m_X = loc.Lng;
m_Y = loc.Lat;
m_H = loc.Elev;
}
public GeoPosition add(GeoPosition a)
{
return(new GeoPosition(m_X + a.X, m_Y + a.Y, m_H + a.H));
}
void canvas_MouseMove(object sender, MouseEventArgs mea)
{
try
{
Point pos = mea.GetPosition(canvasRel);
// first get it in pixels, relative to center point:
double x = pos.X - canvasRel.ActualWidth / 2.0d;
double y = -(pos.Y - canvasRel.ActualHeight / 2.0d);
// and now convert to meters, relative to robot center:
x *= metersPerPixelX;
y *= metersPerPixelY;
MapperVicinity mapperVicinity = CurrentMapper;
GeoPosition gp = new GeoPosition(mapperVicinity.robotPosition);
gp.translate(new Distance(x), new Distance(y));
RoutedEventArgsMouseMoved newEventArgs = new RoutedEventArgsMouseMoved(MapperViewControl.MouseMovedEvent) { xMeters = x, yMeters = y, geoPosition = gp };
RaiseEvent(newEventArgs);
}
catch { }
}
public GeoPosition(GeoPosition loc)
{
m_X = loc.Lng;
m_Y = loc.Lat;
m_H = loc.Elev;
}
public GeoPosition add(GeoPosition a)
{
return new GeoPosition(m_X + a.X, m_Y + a.Y, m_H + a.H);
}
public Distance distanceToWp(GeoPosition myPos)
{
return geoPosition.distanceFrom(myPos);
}
public Direction directionToWp(GeoPosition myPos, Direction myDir)
{
return new Direction() { heading = myDir.heading, bearing = myPos.bearing(this.geoPosition) };
}
public DetectedObstacle(GeoPosition pos)
: base(pos)
{
SetObstacle();
}
public DetectedObstacle(GeoPosition pos)
: base(pos)
{
objectType = DetectedObjectType.Obstacle;
}
public DetectedHuman(GeoPosition pos)
: base(pos)
{
SetHuman();
}
public void setRobotPositionAndDirection(GeoPosition pos, Direction dir)
{
MapperVicinity mapperVicinity = CurrentMapper;
if (pos != null)
{
mapperVicinity.robotPosition = (GeoPosition)pos.Clone();
}
if (dir != null)
{
mapperVicinity.robotDirection = (Direction)dir.Clone();
}
// --------- debug ------------
/*
GeoPosition pos1 = (GeoPosition)pos.Clone();
pos1.translate(new Distance(1.0d), new Distance(1.0d)); // robot coordinates - right forward
DetectedObstacle dobst1 = new DetectedObstacle(pos1) { color = Colors.Red };
mapperVicinity.AddDetectedObject(dobst1);
GeoPosition pos2 = (GeoPosition)pos.Clone();
pos2.translate(new Distance(-1.0d), new Distance(1.0d)); // robot coordinates - left forward
DetectedObstacle dobst2 = new DetectedObstacle(pos2) { color = Colors.Yellow };
mapperVicinity.AddDetectedObject(dobst2);
mapperVicinity.computeMapPositions();
*/
// --------- end debug ------------
RedrawMap();
}
// returns bearing in degrees. To get rads, multiply by Math.PI / 180.0d
public double bearing(GeoPosition nextLoc)
{
double Lon1 = this.Lng * Math.PI / 180.0d;
double Lon2 = nextLoc.Lng * Math.PI / 180.0d;
double Lat1 = this.Lat * Math.PI / 180.0d;
double Lat2 = nextLoc.Lat * Math.PI / 180.0d;
double y = Math.Sin(Lon1-Lon2) * Math.Cos(Lat2);
double x = Math.Cos(Lat1) * Math.Sin(Lat2) - Math.Sin(Lat1) * Math.Cos(Lat2) * Math.Cos(Lon1 - Lon2);
// from http://www.movable-type.co.uk/scripts/LatLong.html
if (Math.Sin(Lon2 - Lon1) > 0.0)
{
return (Math.Atan2(-y, x) * 180.0d / Math.PI);
}
else
{
return (2.0d * Math.PI - Math.Atan2(y, x)) * 180.0d / Math.PI;
}
/*
// see http://www.malaysiagis.com/related_technologies/gps/article3.cfm for the formula and some code
// see http://www.fcaglp.unlp.edu.ar/~esuarez/gmt/1997/0148.html for more
double ret = 0.0d;
double rad_bearing;
double rad_dist = Math.Acos(Math.Sin(Lat1) * Math.Sin(Lat2) + Math.Cos(Lat1) * Math.Cos(Lat2) * Math.Cos(Lon1 - Lon2));
if (Math.Sin(Lon2 - Lon1) > 0.0)
{
double t1 = Math.Sin(Lat2) - Math.Sin(Lat1) * Math.Cos(rad_dist);
double t2 = Math.Cos(Lat1) * Math.Sin(rad_dist);
double t3 = t1 / t2;
double t4 = Math.Atan(-t3 / Math.Sqrt(-t3 * t3 + 1)) + 2 * Math.Atan(1);
rad_bearing = t4;
}
else
{
double t1 = Math.Sin(Lat2) - Math.Sin(Lat1) * Math.Cos(rad_dist);
double t2 = Math.Cos(Lat1) * Math.Sin(rad_dist);
double t3 = t1 / t2;
double t4 = -t3 * t3 + 1;
double t5 = 2.0d * Math.PI - (Math.Atan(-t3 / Math.Sqrt(-t3 * t3 + 1)) + 2 * Math.Atan(1));
rad_bearing = t5;
}
ret = rad_bearing * 180.0d / Math.PI;
return ret;
*/
}
public void moveTo(GeoPosition to)
{
m_X = to.X;
m_Y = to.Y;
m_H = to.H;
}
public Distance distanceFrom(GeoPosition from)
{
// here is a version that is lighter computationally and works well over miles of distance.
// compared to distanceFromExact() the difference is 1mm per meter (0.1%)
double x = m_X - from.X; //double x = this.subtract(from, false).m_X;
double y = m_Y - from.Y; //double y = this.subtract(from, false).m_Y;
// a grad square is cos(latitude) thinner, we need latitude in radians:
double midLatRad = ((from.Y + m_Y) / 2.0d) * Math.PI / 180.0d; //double midLatRad = (this.add(from).m_Y / 2.0d) * Math.PI / 180.0d;
double latitudeFactor = Math.Cos(midLatRad);
double xMeters = Distance.METERS_PER_DEGREE * x * latitudeFactor;
double yMeters = Distance.METERS_PER_DEGREE * y;
double meters = Math.Sqrt(xMeters * xMeters + yMeters * yMeters);
Distance distance = new Distance(meters);
return distance;
}
public Distance distanceFromExact(GeoPosition from)
{
// from http://www.movable-type.co.uk/scripts/LatLong.html
double lon1 = this.Lng * Math.PI / 180.0d;
double lon2 = from.Lng * Math.PI / 180.0d;
double lat1 = this.Lat * Math.PI / 180.0d;
double lat2 = from.Lat * Math.PI / 180.0d;
double dLat = lat2 - lat1;
double dLong = lon2 - lon1;
double a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) + Math.Cos(lat1) * Math.Cos(lat2) * Math.Sin(dLong / 2) * Math.Sin(dLong / 2);
double c = 2.0d * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1.0d - a));
double meters = EARTH_RADIUS * c;
Distance distance = new Distance(meters);
return distance;
}
public void moveTo(GeoPosition to)
{
m_X = to.X;
m_Y = to.Y;
m_H = to.H;
}
public DetectedObjectBase(GeoPosition pos)
: this()
{
geoPosition = (GeoPosition)pos.Clone();
}
public GeoPosition subtract(GeoPosition a, bool spans180)
{
double x = a.X;
double dx = m_X - x;
if(spans180)
{ // dx < 360.0 && Math.Abs(dx) > 180.0) {
if(x > 90.0 && m_X < -90)
{
x -= 360.0;
}
else if(m_X > 90.0 && x < -90)
{
x += 360.0;
}
dx = m_X - x;
}
double dy = m_Y - a.Y;
double dz = m_H - a.H;
return new GeoPosition(dx, dy, dz);
}
public DetectedObjectBase(GeoPosition pos)
: this()
{
geoPosition = (GeoPosition)pos.Clone();
}
public DetectedHuman(GeoPosition pos)
: base(pos)
{
SetHuman();
}
public Distance distanceTo(GeoPosition myPos)
{
return(this.geoPosition.distanceFrom(myPos));
}
public Distance distanceTo(GeoPosition myPos)
{
return this.geoPosition.distanceFrom(myPos);
}
