133 lines
4.9 KiB
C#
133 lines
4.9 KiB
C#
/*
|
|
* Copyright 2016 The Cartographer Authors
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
using CartographerSharp.Common.Math;
|
|
using CartographerSharp.Models.Transform;
|
|
using RobotNet10.Shared.Numbers;
|
|
|
|
namespace CartographerSharp.Mapping.D2D;
|
|
|
|
/// <summary>
|
|
/// Defines the limits of a grid map.
|
|
/// </summary>
|
|
public class MapLimits
|
|
{
|
|
private readonly double _resolution;
|
|
private readonly Vector2 _max;
|
|
private readonly CellLimits _cellLimits;
|
|
|
|
public MapLimits(double resolution, Vector2 max, CellLimits cellLimits)
|
|
{
|
|
if (resolution <= 0.0)
|
|
{
|
|
throw new ArgumentException("Resolution must be positive", nameof(resolution));
|
|
}
|
|
if (cellLimits.NumXCells <= 0 || cellLimits.NumYCells <= 0)
|
|
{
|
|
throw new ArgumentException("Cell limits must be positive", nameof(cellLimits));
|
|
}
|
|
|
|
_resolution = resolution;
|
|
_max = max;
|
|
_cellLimits = cellLimits;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates from proto representation.
|
|
/// Match C++: explicit MapLimits(const proto::MapLimits& map_limits)
|
|
/// </summary>
|
|
public MapLimits(Models.Mapping.MapLimits proto)
|
|
{
|
|
_resolution = proto.Resolution;
|
|
_max = new Vector2(proto.Max.X, proto.Max.Y);
|
|
// Match C++: cell_limits_(map_limits.cell_limits())
|
|
_cellLimits = new CellLimits(proto.CellLimits);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns the cell size in meters. All cells are square and the resolution is
|
|
/// the length of one side.
|
|
/// </summary>
|
|
public double Resolution => _resolution;
|
|
|
|
/// <summary>
|
|
/// Returns the corner of the limits, i.e., all pixels have positions with
|
|
/// smaller coordinates.
|
|
/// </summary>
|
|
public Vector2 Max => _max;
|
|
|
|
/// <summary>
|
|
/// Returns the limits of the grid in number of cells.
|
|
/// </summary>
|
|
public CellLimits CellLimits => _cellLimits;
|
|
|
|
/// <summary>
|
|
/// Returns the index of the cell containing the 'point' which may be outside
|
|
/// the map, i.e., negative or too large indices that will return false for
|
|
/// Contains().
|
|
/// </summary>
|
|
public Array2i GetCellIndex(Vector2 point)
|
|
{
|
|
// Index values are row major and the top left has (0, 0)
|
|
// and contains (centered_max_x, centered_max_y). We need to flip and rotate.
|
|
// Match C++: common::RoundToInt(x) = std::lround(x) - rounds to nearest long, then casts to int
|
|
// std::lround rounds to nearest integer using current rounding mode (default: round half away from zero)
|
|
// In C#, Math.Round with MidpointRounding.AwayFromZero matches std::lround behavior
|
|
// CRITICAL: Use std::lround equivalent - Math.Round with MidpointRounding.AwayFromZero
|
|
// But std::lround actually uses "round half to even" (banker's rounding) by default in C++11+
|
|
// However, C++ code uses common::RoundToInt which is std::lround, and std::lround uses current rounding mode
|
|
// In practice, std::lround with default rounding mode rounds half away from zero
|
|
// So Math.Round with MidpointRounding.AwayFromZero should match
|
|
var x = (int)Math.Round((_max.Y - point.Y) / _resolution - 0.5, MidpointRounding.AwayFromZero);
|
|
var y = (int)Math.Round((_max.X - point.X) / _resolution - 0.5, MidpointRounding.AwayFromZero);
|
|
return new Array2i(x, y);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns the center of the cell at 'cell_index'.
|
|
/// </summary>
|
|
public Vector2 GetCellCenter(Array2i cellIndex)
|
|
{
|
|
return new Vector2(
|
|
(_max.X - Resolution * (cellIndex.Y + 0.5)),
|
|
(_max.Y - Resolution * (cellIndex.X + 0.5))
|
|
);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns true if the grid contains 'cell_index'.
|
|
/// </summary>
|
|
public bool Contains(Array2i cellIndex)
|
|
{
|
|
return cellIndex.X >= 0 && cellIndex.Y >= 0 &&
|
|
cellIndex.X < _cellLimits.NumXCells &&
|
|
cellIndex.Y < _cellLimits.NumYCells;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts to proto representation.
|
|
/// </summary>
|
|
public Models.Mapping.MapLimits ToProto()
|
|
{
|
|
var cellLimitsProto = _cellLimits.ToProto();
|
|
return new Models.Mapping.MapLimits(
|
|
_resolution,
|
|
new Vector2d(_max.X, _max.Y),
|
|
new Models.Mapping.CellLimits(cellLimitsProto.NumXCells, cellLimitsProto.NumYCells)
|
|
);
|
|
}
|
|
}
|