public DetectedObjectBase(Direction dir, Distance dist)
: this()
{
relPosition = new RelPosition(dir, dist);
}
public DetectedObstacle(Direction dir, Distance dist)
: base(dir, dist)
{
objectType = DetectedObjectType.Obstacle;
}
// converts distance along the longitude line (Y) into degrees
public double toDegreesY(Distance d)
{
// vertical degrees are the same always:
return(d.Meters / Distance.METERS_PER_DEGREE);
}
public DetectedObstacle(Direction dir, Distance dist)
: base(dir, dist)
{
SetObstacle();
}
public DetectedHuman(Direction dir, Distance dist)
: base(dir, dist)
{
SetHuman();
}
// works only for small distances, within miles.
public void translate(Distance byX, Distance byY)
{
m_X += toDegreesX(byX);
m_Y += toDegreesY(byY);
}
// works only for small distances, within miles. For general case - when bearing is missing, or when we need to move towards bearing.
public void translate(Direction dir, Distance by)
{
double range = by.Meters;
double angle = (double)dir.heading; // degrees
if (dir.bearing.HasValue)
{
// if both heading and bearing are supplied, then heading represents robot direction, and bearing - absolute direction to the object.
// we are interested in translating in the "bearing" direction in this case, it is done to the objects relative to the robot.
angle = (double)dir.bearing;
}
m_X += toDegreesX(new Distance(range * Math.Sin(angle * Math.PI / 180.0d)));
m_Y += toDegreesY(new Distance(range * Math.Cos(angle * Math.PI / 180.0d)));
}
// works only for small distances, within miles. For rare cases when we have both heading and bearing, but want to move in the "heading" direction.
public void translateToHeading(Direction dir, Distance by)
{
double range = by.Meters;
double angle = (double)dir.heading; // degrees
m_X += toDegreesX(new Distance(range * Math.Sin(angle * Math.PI / 180.0d)));
m_Y += toDegreesY(new Distance(range * Math.Cos(angle * Math.PI / 180.0d)));
}
// converts distance along the longitude line (Y) into degrees
public double toDegreesY(Distance d)
{
// vertical degrees are the same always:
return d.Meters / Distance.METERS_PER_DEGREE;
}
// works only for small distances, within miles.
public void translate(Distance byX, Distance byY)
{
m_X += toDegreesX(byX);
m_Y += toDegreesY(byY);
}
// converts distance along the latitude line (X) into degrees
public double toDegreesX(Distance d)
{
// horizontal degrees are shorter in meters as we go up the latitude:
double latitudeFactor = Math.Cos(this.Lat * Math.PI / 180.0d);
return d.Meters / Distance.METERS_PER_DEGREE / latitudeFactor;
}
public string heightToString()
{
string height = "";
if(m_H > 0.3d) // meters
{
Distance d = new Distance(m_H);
height = " " + d.ToStringCompl() + " high";
}
else if(m_H < -0.3d) // meters
{
Distance d = new Distance(-m_H);
height = " " + d + " deep";
}
return height;
}
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 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 DetectedObstacle(Direction dir, Distance dist)
: base(dir, dist)
{
SetObstacle();
}