QUYVN/BQP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

239
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/RayToPixelMask.cs Normal file
View File

@@ -0,0 +1,239 @@
/*
* 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}");
}
}
}