240 lines
8.9 KiB
C#
240 lines
8.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;
|
|
|
|
namespace CartographerSharp.Mapping.Internal.D2D;
|
|
|
|
/// <summary>
|
|
/// Ray to pixel mask utilities.
|
|
/// </summary>
|
|
public static class RayToPixelMask
|
|
{
|
|
/// <summary>
|
|
/// Compute all pixels that contain some part of the line segment connecting
|
|
/// 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled
|
|
/// by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be
|
|
/// greater than zero. Return values are in pixels and not scaled.
|
|
/// </summary>
|
|
public static List<Array2i> Compute(
|
|
Array2i scaledBegin,
|
|
Array2i scaledEnd,
|
|
int subpixelScale)
|
|
{
|
|
var result = new List<Array2i>();
|
|
ComputeInto(scaledBegin, scaledEnd, subpixelScale, result);
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Same as Compute but appends results into an existing list (caller must Clear beforehand).
|
|
/// This avoids per-ray List allocation when processing many rays in a loop.
|
|
/// </summary>
|
|
public static void ComputeInto(
|
|
Array2i scaledBegin,
|
|
Array2i scaledEnd,
|
|
int subpixelScale,
|
|
List<Array2i> pixelMask)
|
|
{
|
|
// For simplicity, we order 'scaled_begin' and 'scaled_end' by their x
|
|
// coordinate.
|
|
if (scaledBegin.X > scaledEnd.X)
|
|
{
|
|
ComputeInto(scaledEnd, scaledBegin, subpixelScale, pixelMask);
|
|
return;
|
|
}
|
|
|
|
// Match C++ CHECK_GE assertions for all coordinates
|
|
if (scaledBegin.X < 0 || scaledBegin.Y < 0 || scaledEnd.X < 0 || scaledEnd.Y < 0)
|
|
{
|
|
throw new ArgumentException("Scaled coordinates must be non-negative");
|
|
}
|
|
|
|
// Track last added element to avoid consecutive duplicates (avoids pixelMask[^1] indexer overhead)
|
|
int lastX = int.MinValue, lastY = int.MinValue;
|
|
|
|
// Special case: We have to draw a vertical line in full pixels, as
|
|
// 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate.
|
|
if (scaledBegin.X / subpixelScale == scaledEnd.X / subpixelScale)
|
|
{
|
|
var cx = scaledBegin.X / subpixelScale;
|
|
var cy = Math.Min(scaledBegin.Y, scaledEnd.Y) / subpixelScale;
|
|
pixelMask.Add(new Array2i(cx, cy));
|
|
lastX = cx; lastY = cy;
|
|
var endY = Math.Max(scaledBegin.Y, scaledEnd.Y) / subpixelScale;
|
|
for (; cy <= endY; cy++)
|
|
{
|
|
if (cx != lastX || cy != lastY)
|
|
{
|
|
pixelMask.Add(new Array2i(cx, cy));
|
|
lastX = cx; lastY = cy;
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
|
|
// Match C++ int64 types to prevent integer overflow
|
|
long dx = (long)scaledEnd.X - scaledBegin.X;
|
|
long dy = (long)scaledEnd.Y - scaledBegin.Y;
|
|
long denominator = 2L * subpixelScale * dx;
|
|
|
|
// The current full pixel coordinates. We start at 'scaled_begin'.
|
|
int curX = scaledBegin.X / subpixelScale;
|
|
int curY = scaledBegin.Y / subpixelScale;
|
|
pixelMask.Add(new Array2i(curX, curY));
|
|
lastX = curX; lastY = curY;
|
|
|
|
// The center of the subpixel part of 'scaled_begin.y()' assuming the
|
|
// 'denominator', i.e., sub_y / denominator is in (0, 1).
|
|
long subY = (2L * (scaledBegin.Y % subpixelScale) + 1) * dx;
|
|
|
|
// The distance from the from 'scaled_begin' to the right pixel border, to be
|
|
// divided by 2 * 'subpixel_scale'.
|
|
long firstPixel = 2L * subpixelScale - 2L * (scaledBegin.X % subpixelScale) - 1;
|
|
// The same from the left pixel border to 'scaled_end'.
|
|
long lastPixel = 2L * (scaledEnd.X % subpixelScale) + 1;
|
|
|
|
// The full pixel x coordinate of 'scaled_end'.
|
|
var endX = Math.Max(scaledBegin.X, scaledEnd.X) / subpixelScale;
|
|
|
|
// Move from 'scaled_begin' to the next pixel border to the right.
|
|
subY += dy * firstPixel;
|
|
|
|
if (dy > 0)
|
|
{
|
|
while (true)
|
|
{
|
|
if (curX != lastX || curY != lastY)
|
|
{
|
|
pixelMask.Add(new Array2i(curX, curY));
|
|
lastX = curX; lastY = curY;
|
|
}
|
|
while (subY > denominator)
|
|
{
|
|
subY -= denominator;
|
|
curY++;
|
|
if (curX != lastX || curY != lastY)
|
|
{
|
|
pixelMask.Add(new Array2i(curX, curY));
|
|
lastX = curX; lastY = curY;
|
|
}
|
|
}
|
|
curX++;
|
|
if (subY == denominator)
|
|
{
|
|
subY -= denominator;
|
|
curY++;
|
|
}
|
|
if (curX == endX)
|
|
{
|
|
break;
|
|
}
|
|
// Move from one pixel border to the next.
|
|
subY += dy * 2L * subpixelScale;
|
|
}
|
|
// Move from the pixel border on the right to 'scaled_end'.
|
|
subY += dy * lastPixel;
|
|
|
|
if (curX != lastX || curY != lastY)
|
|
{
|
|
pixelMask.Add(new Array2i(curX, curY));
|
|
lastX = curX; lastY = curY;
|
|
}
|
|
while (subY > denominator)
|
|
{
|
|
subY -= denominator;
|
|
curY++;
|
|
if (curX != lastX || curY != lastY)
|
|
{
|
|
pixelMask.Add(new Array2i(curX, curY));
|
|
lastX = curX; lastY = curY;
|
|
}
|
|
}
|
|
|
|
// Match C++ CHECK_NE(sub_y, denominator) - subY should not equal denominator
|
|
if (subY == denominator)
|
|
{
|
|
throw new InvalidOperationException("subY should not equal denominator");
|
|
}
|
|
|
|
// Match C++ CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale)
|
|
var expectedY = scaledEnd.Y / subpixelScale;
|
|
if (curY != expectedY)
|
|
{
|
|
throw new InvalidOperationException($"Current y should equal scaledEnd.Y / subpixelScale: current.Y={curY}, scaledEnd.Y/subpixelScale={expectedY}, scaledBegin=({scaledBegin.X}, {scaledBegin.Y}), scaledEnd=({scaledEnd.X}, {scaledEnd.Y}), subpixelScale={subpixelScale}");
|
|
}
|
|
return;
|
|
}
|
|
|
|
// Same for lines non-ascending in y coordinates.
|
|
while (true)
|
|
{
|
|
if (curX != lastX || curY != lastY)
|
|
{
|
|
pixelMask.Add(new Array2i(curX, curY));
|
|
lastX = curX; lastY = curY;
|
|
}
|
|
while (subY < 0)
|
|
{
|
|
subY += denominator;
|
|
curY--;
|
|
if (curX != lastX || curY != lastY)
|
|
{
|
|
pixelMask.Add(new Array2i(curX, curY));
|
|
lastX = curX; lastY = curY;
|
|
}
|
|
}
|
|
curX++;
|
|
if (subY == 0)
|
|
{
|
|
subY += denominator;
|
|
curY--;
|
|
}
|
|
if (curX == endX)
|
|
{
|
|
break;
|
|
}
|
|
subY += dy * 2L * subpixelScale;
|
|
}
|
|
subY += dy * lastPixel;
|
|
if (curX != lastX || curY != lastY)
|
|
{
|
|
pixelMask.Add(new Array2i(curX, curY));
|
|
lastX = curX; lastY = curY;
|
|
}
|
|
while (subY < 0)
|
|
{
|
|
subY += denominator;
|
|
curY--;
|
|
if (curX != lastX || curY != lastY)
|
|
{
|
|
pixelMask.Add(new Array2i(curX, curY));
|
|
lastX = curX; lastY = curY;
|
|
}
|
|
}
|
|
// Match C++: CHECK_NE(sub_y, 0)
|
|
if (subY == 0)
|
|
{
|
|
throw new InvalidOperationException("subY should not equal 0");
|
|
}
|
|
// Match C++: CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale)
|
|
if (curY != scaledEnd.Y / subpixelScale)
|
|
{
|
|
throw new InvalidOperationException($"Current y should equal scaledEnd.y / subpixelScale: current.Y={curY}, scaledEnd.Y/subpixelScale={scaledEnd.Y / subpixelScale}, scaledBegin=({scaledBegin.X}, {scaledBegin.Y}), scaledEnd=({scaledEnd.X}, {scaledEnd.Y}), subpixelScale={subpixelScale}");
|
|
}
|
|
}
|
|
}
|