public RobotPoseBase()
{
geoPosition = new GeoPosition(0.0d, 0.0d);
direction = new Direction();
H = 0.0d;
}
public Direction dir; // straight forward is 0, right is positive
public RelPosition(Direction direction, Distance distance)
{
this.dir = (Direction)direction.Clone();
this.dist = (Distance)distance.Clone();
if (direction.bearingRelative.HasValue)
{
double bearingRad = DirectionMath.toRad(direction.bearingRelative.Value);
X = distance.Meters * Math.Sin(bearingRad);
Y = -distance.Meters * Math.Cos(bearingRad);
}
else if (direction.bearing.HasValue)
{
double bearingRad = DirectionMath.toRad(direction.bearing.Value);
X = distance.Meters * Math.Sin(bearingRad);
Y = -distance.Meters * Math.Cos(bearingRad);
}
}
