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); } }