577 lines
19 KiB
C#
577 lines
19 KiB
C#
using Masuit.Tools;
|
||
|
||
namespace DispenserCommon.Utils;
|
||
|
||
/// <summary>
|
||
/// 数组工具类
|
||
/// </summary>
|
||
public class CoordinateUtil
|
||
{
|
||
/// <summary>
|
||
/// 将一个二维坐标数组拆分成两个一维数组
|
||
/// </summary>
|
||
/// <param name="coordinates">二维坐标数组</param>
|
||
/// <param name="xAxis">X轴坐标数组</param>
|
||
/// <param name="yAxis">Y轴坐标数组</param>
|
||
public static void SplitCoordinates(double[][] coordinates, out double[] xAxis, out double[] yAxis)
|
||
{
|
||
var totalPoints = coordinates.GetLength(0);
|
||
xAxis = new double[totalPoints];
|
||
yAxis = new double[totalPoints];
|
||
|
||
for (var i = 0; i < totalPoints; i++)
|
||
{
|
||
// 提取x坐标
|
||
xAxis[i] = coordinates[i][0];
|
||
// 提取y坐标
|
||
yAxis[i] = coordinates[i][1];
|
||
}
|
||
}
|
||
|
||
public static void SplitCoordinates(double[][] coordinates, out double[] xAxis, out double[] yAxis,
|
||
out double[] depth)
|
||
{
|
||
var totalPoints = coordinates.GetLength(0);
|
||
xAxis = new double[totalPoints];
|
||
yAxis = new double[totalPoints];
|
||
depth = new double[totalPoints];
|
||
|
||
for (var i = 0; i < totalPoints; i++)
|
||
{
|
||
// 提取x坐标
|
||
xAxis[i] = coordinates[i][0];
|
||
// 提取y坐标
|
||
yAxis[i] = coordinates[i][1];
|
||
// 提取针刺深度
|
||
depth[i] = coordinates[i][3];
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将两个坐标数组合并成一个二维坐标数组
|
||
/// </summary>
|
||
/// <param name="coordinates1"></param>
|
||
/// <param name="coordinates2"></param>
|
||
/// <returns></returns>
|
||
public static double[][] MergeArray(IEnumerable<double> coordinates1, double[] coordinates2)
|
||
{
|
||
List<double[]> merged = [];
|
||
merged.AddRange(coordinates1.Select((t, i) => (double[]) [t, coordinates2[i]]));
|
||
|
||
return merged.ToArray();
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 判断数组是否为空
|
||
/// </summary>
|
||
/// <param name="array"></param>
|
||
/// <returns></returns>
|
||
public static bool IsEmpty(double[]? array)
|
||
{
|
||
return array == null || !array.Any();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断数组是否为空
|
||
/// </summary>
|
||
/// <param name="array"></param>
|
||
/// <returns></returns>
|
||
public static bool IsEmpty(double[][]? array)
|
||
{
|
||
return array == null || !array.Any();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断数组是否为空
|
||
/// </summary>
|
||
/// <param name="array"></param>
|
||
/// <returns></returns>
|
||
public static bool IsEmpty(double[,]? array)
|
||
{
|
||
return array == null || array.GetLength(0) == 0 || array.GetLength(1) == 0;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 合并相似点
|
||
/// </summary>
|
||
/// <param name="points"></param>
|
||
/// <param name="window">相似划分窗口</param>
|
||
/// <param name="rowSimilarPitchThreshold">行相似值判断阈值</param>
|
||
/// <param name="colSimilarPitchThreshold">列相似值判断阈值</param>
|
||
/// <returns></returns>
|
||
public static List<double[]> MergeSimilarPoints(List<double[]> points, string window,
|
||
double rowSimilarPitchThreshold,
|
||
double colSimilarPitchThreshold)
|
||
{
|
||
var windowConfig = window;
|
||
var windows = windowConfig.Split(",");
|
||
points = windows
|
||
.Select(int.Parse)
|
||
.Aggregate(points,
|
||
(current, windowSize) =>
|
||
MergeSimilarPoints(current, windowSize, rowSimilarPitchThreshold, colSimilarPitchThreshold));
|
||
|
||
return points;
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 对坐标进行分区计算，提高计算效率
|
||
/// </summary>
|
||
/// <param name="points">合并前的坐标点</param>
|
||
/// <param name="space">分区数</param>
|
||
/// <returns></returns>
|
||
private static List<List<List<double[]>>> DivideIntoRegions(List<double[]> points, int space)
|
||
{
|
||
// 找到坐标范围
|
||
var minX = points.Min(p => p[0]);
|
||
var maxX = points.Max(p => p[0]);
|
||
var minY = points.Min(p => p[1]);
|
||
var maxY = points.Max(p => p[1]);
|
||
|
||
// 计算每个区域的尺寸
|
||
var regionWidth = (maxX - minX) / space;
|
||
var regionHeight = (maxY - minY) / space;
|
||
|
||
// 初始化区域列表
|
||
var regions = new List<List<List<double[]>>>(space);
|
||
for (var i = 0; i < space; i++)
|
||
{
|
||
regions.Add(new List<List<double[]>>(space));
|
||
for (var j = 0; j < space; j++) regions[i].Add([]);
|
||
}
|
||
|
||
// 将点分配到相应的区域
|
||
foreach (var point in points)
|
||
{
|
||
var x = point[0];
|
||
var y = point[1];
|
||
var row = (int)Math.Floor((y - minY) / regionHeight);
|
||
var col = (int)Math.Floor((x - minX) / regionWidth);
|
||
|
||
// 防止由于边界值导致的索引超出范围
|
||
row = Math.Min(row, space - 1);
|
||
col = Math.Min(col, space - 1);
|
||
|
||
regions[row][col].Add(point);
|
||
}
|
||
|
||
return regions;
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 合并相似点
|
||
/// </summary>
|
||
/// <param name="points">合并前的坐标点集合</param>
|
||
/// <param name="space">区域数</param>
|
||
/// <param name="rowSimilarPitchThreshold">行相似值判断阈值</param>
|
||
/// <param name="colSimilarPitchThreshold">列相似值判断阈值</param>
|
||
/// <returns></returns>
|
||
private static List<double[]> MergeSimilarPoints(List<double[]> points, int space, double rowSimilarPitchThreshold,
|
||
double colSimilarPitchThreshold)
|
||
{
|
||
// 将整个区域划分为 若干个区域
|
||
var regions = DivideIntoRegions(points, space);
|
||
|
||
var merged = new List<double[]>();
|
||
// 遍历每个区域，对每个区域进行相似点合并
|
||
foreach (var region in regions)
|
||
foreach (var col in region)
|
||
merged.AddRange(MergePoints(col, rowSimilarPitchThreshold, colSimilarPitchThreshold));
|
||
|
||
return merged;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 对区域内的点进行合并处理
|
||
/// </summary>
|
||
/// <param name="points">待合并区域</param>
|
||
/// <param name="rowSimilarPitchThreshold">行相似值判断阈值</param>
|
||
/// <param name="colSimilarPitchThreshold">列相似值判断阈值</param>
|
||
/// <returns></returns>
|
||
private static List<double[]> MergePoints(List<double[]> points, double rowSimilarPitchThreshold,
|
||
double colSimilarPitchThreshold)
|
||
{
|
||
if (points.Count == 0)
|
||
return new List<double[]>();
|
||
|
||
// 如果两个两个点的 abs(x2-x1) <= 0.2 && abs(y2-y1) <= 0.35, 那么可以认为这两个点是相似的,只保留第一个点即可
|
||
var merged = new List<double[]>();
|
||
foreach (var point in points)
|
||
// 同当前区域的其他点进行比较
|
||
if (merged.Count == 0)
|
||
{
|
||
merged.Add(point);
|
||
}
|
||
else
|
||
{
|
||
var similar = false;
|
||
foreach (var other in merged)
|
||
if (IsSimilar(point, other, 0.1, 0.1))
|
||
{
|
||
Console.WriteLine(
|
||
$"{point[0]},{point[1]} 与 {other[0]},{other[1]} 相似, X差值{point[0] - other[0]}， y差值{point[1] - other[1]}");
|
||
similar = true;
|
||
break;
|
||
}
|
||
|
||
if (!similar) merged.Add(point);
|
||
}
|
||
|
||
return merged;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断两个点是否相似
|
||
/// </summary>
|
||
/// <param name="p1">第一个点</param>
|
||
/// <param name="p2">第二个点</param>
|
||
/// <param name="rowSimilarPitchThreshold">行相似值判断阈值</param>
|
||
/// <param name="colSimilarPitchThreshold">列相似值判断阈值</param>
|
||
/// <returns></returns>
|
||
private static bool IsSimilar(double[] p1, double[] p2, double rowSimilarPitchThreshold,
|
||
double colSimilarPitchThreshold)
|
||
{
|
||
return Math.Abs(p2[0] - p1[0]) <= rowSimilarPitchThreshold &&
|
||
Math.Abs(p2[1] - p1[1]) <= colSimilarPitchThreshold;
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 从原数组中根据匹配点数组查找相似点
|
||
/// </summary>
|
||
/// <param name="referencePoints"></param>
|
||
/// <param name="comparisonPoints"></param>
|
||
/// <param name="rowSimilarPitchThreshold">行相似值判断阈值</param>
|
||
/// <param name="colSimilarPitchThreshold">列相似值判断阈值</param>
|
||
/// <returns></returns>
|
||
public static List<double[]> FindSimilarPoints(List<double[]> referencePoints, List<double[]> comparisonPoints,
|
||
double rowSimilarPitchThreshold,
|
||
double colSimilarPitchThreshold)
|
||
{
|
||
// 从原数组中根据匹配点数组查找相似点
|
||
return referencePoints.AsParallel().Where(sourcePoint =>
|
||
comparisonPoints.Any(matchPoint =>
|
||
IsSimilar(sourcePoint, matchPoint, rowSimilarPitchThreshold, colSimilarPitchThreshold))
|
||
).ToList();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 晶环和pcb坐标长度对齐
|
||
/// </summary>
|
||
/// <param name="pcbCoordinates">原始pcb坐标</param>
|
||
/// <param name="waferCoordinates">原始晶环坐标</param>
|
||
/// <param name="pcbAlignedCoordinates">对齐后的pcb坐标</param>
|
||
/// <param name="waferAlignedCoordinates">对齐后的晶环坐标</param>
|
||
public static void CoordinateAlign(List<double[]> pcbCoordinates, List<double[]> waferCoordinates,
|
||
out List<double[]> pcbAlignedCoordinates, out List<double[]> waferAlignedCoordinates)
|
||
{
|
||
pcbAlignedCoordinates = [];
|
||
waferAlignedCoordinates = [];
|
||
var pcbStartIndex = 0;
|
||
var waferStartIndex = 0;
|
||
var pStart = 0;
|
||
var wStart = 0;
|
||
while (pcbStartIndex <= pcbCoordinates.Count && waferStartIndex <= waferCoordinates.Count)
|
||
if ((IsReal(pcbCoordinates, pcbStartIndex) && IsReal(waferCoordinates, waferStartIndex)) ||
|
||
(!IsReal(pcbCoordinates, pcbStartIndex) && !IsReal(waferCoordinates, waferStartIndex)))
|
||
{
|
||
pcbAlignedCoordinates[pStart++] = pcbCoordinates[pcbStartIndex++];
|
||
waferAlignedCoordinates[wStart++] = waferCoordinates[waferStartIndex++];
|
||
}
|
||
else if (IsReal(pcbCoordinates, pcbStartIndex) && !IsReal(waferCoordinates, waferStartIndex))
|
||
{
|
||
var pcbCoordinate = new double[3];
|
||
Array.Copy(pcbCoordinates[pcbStartIndex], pcbCoordinate, 3);
|
||
pcbCoordinate[2] = 0;
|
||
pcbAlignedCoordinates[pStart++] = pcbCoordinate;
|
||
waferAlignedCoordinates[wStart++] = waferCoordinates[waferStartIndex++];
|
||
}
|
||
else
|
||
{
|
||
pcbAlignedCoordinates[pStart++] = pcbCoordinates[pcbStartIndex++];
|
||
var waferCoordinate = new double[3];
|
||
Array.Copy(waferCoordinates[waferStartIndex], waferCoordinate, 3);
|
||
waferCoordinate[2] = 0;
|
||
waferAlignedCoordinates[wStart++] = waferCoordinate;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断是否是真实坐标
|
||
/// </summary>
|
||
/// <param name="pcbCoordinates"></param>
|
||
/// <param name="pcbStartIndex"></param>
|
||
/// <returns></returns>
|
||
private static bool IsReal(IReadOnlyList<double[]> pcbCoordinates, int pcbStartIndex)
|
||
{
|
||
return pcbCoordinates[pcbStartIndex][2] != 0;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从插值路径中提取动打坐标信息
|
||
/// </summary>
|
||
/// <param name="coordinates">插值后的坐标数组</param>
|
||
/// <param name="startPoint">开始的坐标</param>
|
||
/// <param name="endPoint">借宿的坐标</param>
|
||
public static List<double[]> SubCurrentBatchCoordinate(IEnumerable<double[]> coordinates, double[] startPoint,
|
||
double[] endPoint)
|
||
{
|
||
// 保存当前截取后的新坐标
|
||
List<double[]> newCoordinates = [];
|
||
var target = false;
|
||
|
||
foreach (var coordinate in coordinates)
|
||
{
|
||
if (Equals(coordinate, startPoint))
|
||
{
|
||
target = true;
|
||
}
|
||
else if (Equals(coordinate, endPoint))
|
||
{
|
||
newCoordinates.Add(coordinate);
|
||
break;
|
||
}
|
||
|
||
if (target)
|
||
// 都是目标值
|
||
newCoordinates.Add(coordinate);
|
||
}
|
||
|
||
return newCoordinates;
|
||
}
|
||
|
||
public static bool Equals(double[] a, double[] b)
|
||
{
|
||
if (IsEmpty(a) || IsEmpty(b)) return false;
|
||
|
||
return Math.Abs(a[0] - b[0]) < 0.00001 && Math.Abs(a[1] - b[1]) < 0.00001;
|
||
}
|
||
|
||
public static double[][] FillDefaultFlagIfNotExist(double[][] path, double defaultFlag = 1)
|
||
{
|
||
if (path.IsNullOrEmpty()) return path;
|
||
if (path[0].Length == 2) return path.Select(v => new[] { v[0], v[1], defaultFlag }).ToArray();
|
||
|
||
return path;
|
||
}
|
||
|
||
public static bool IsRealPoint(double flag)
|
||
{
|
||
return Math.Abs(Math.Round(flag, MidpointRounding.ToEven) - 1) < 0.0001;
|
||
}
|
||
|
||
public static double GetTowCoordinatesDistinct(double[] p1, double[] p2)
|
||
{
|
||
return Math.Sqrt(Math.Pow(p1[0] - p2[0], 2) + Math.Pow(p1[1] - p2[1], 2));
|
||
}
|
||
|
||
public static double[][] SubPcbPath(double[][] path, int rowAmount, int startColumn, int pathRowAmount)
|
||
{
|
||
List<double[]> result = new();
|
||
|
||
if ((startColumn + pathRowAmount - 1) > rowAmount)
|
||
{
|
||
pathRowAmount = rowAmount - startColumn + 1;
|
||
}
|
||
|
||
var rows = RowBy(path.ToList(), rowAmount);
|
||
|
||
for (var i = 0; i < rows.Count; i++)
|
||
{
|
||
var row = rows[i];
|
||
List<double[]> chunkRow;
|
||
if (i % 2 == 0)
|
||
{
|
||
chunkRow = row.Skip(startColumn - 1).Take(pathRowAmount).ToList();
|
||
}
|
||
else
|
||
{
|
||
chunkRow = row.Skip(rowAmount - (pathRowAmount + startColumn - 1)).Take(pathRowAmount).ToList();
|
||
}
|
||
|
||
result.AddRange(chunkRow);
|
||
}
|
||
|
||
return result.ToArray();
|
||
}
|
||
|
||
static List<List<double[]>> RowBy(List<double[]> source, int chunkSize)
|
||
{
|
||
List<List<double[]>> result = new List<List<double[]>>();
|
||
|
||
for (int i = 0; i < source.Count; i += chunkSize)
|
||
{
|
||
result.Add(source.GetRange(i, Math.Min(chunkSize, source.Count - i)));
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
public static double[] AffineTransform(double[,] matrix, IReadOnlyList<double> point)
|
||
{
|
||
if (matrix.GetLength(0) != 3 || matrix.GetLength(1) != 3)
|
||
throw new ArgumentException("Matrix must be a 3x3 array.");
|
||
|
||
return new double[]
|
||
{
|
||
matrix[0, 0] * point[0] + matrix[0, 1] * point[1] + matrix[0, 2] * point[2],
|
||
matrix[1, 0] * point[0] + matrix[1, 1] * point[1] + matrix[1, 2] * point[2]
|
||
};
|
||
}
|
||
|
||
public static double[][] Convert2DArrayToJaggedArray(double[,] twoDArray)
|
||
{
|
||
int rows = twoDArray.GetLength(0); // 获取第一维度的长度，即行数
|
||
int cols = twoDArray.GetLength(1); // 获取第二维度的长度，即列数
|
||
|
||
double[][] jaggedArray = new double[rows][]; // 创建外层数组
|
||
|
||
for (int i = 0; i < rows; i++)
|
||
{
|
||
jaggedArray[i] = new double[cols]; // 每一行创建一个新的内层数组
|
||
for (int j = 0; j < cols; j++)
|
||
{
|
||
jaggedArray[i][j] = twoDArray[i, j]; // 复制元素
|
||
}
|
||
}
|
||
|
||
return jaggedArray;
|
||
}
|
||
|
||
public static List<List<double[]>> GroupPointsIntoRows(double[][] pointsArray, double yThreshold)
|
||
{
|
||
// 对点数组进行排序
|
||
var pointsArraySorted = pointsArray.OrderBy(p => p[0]).ToArray();
|
||
List<List<double[]>> rows = new List<List<double[]>>();
|
||
|
||
foreach (var point in pointsArraySorted)
|
||
{
|
||
bool placed = false;
|
||
foreach (var row in rows)
|
||
{
|
||
if (Math.Abs(row.Last()[1] - point[1]) <= yThreshold && point[0] > row.Last()[0])
|
||
{
|
||
row.Add(point);
|
||
placed = true;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (!placed)
|
||
{
|
||
rows.Add(new List<double[]> { point });
|
||
}
|
||
}
|
||
|
||
return rows.OrderBy(p => p[0][1]).ToList();
|
||
}
|
||
|
||
public static List<double[]> ConvertMatrixToList(double[,] matrix)
|
||
{
|
||
int rows = matrix.GetLength(0);
|
||
int columns = matrix.GetLength(1);
|
||
var list = new List<double[]>();
|
||
|
||
for (int i = 0; i < rows; i++)
|
||
{
|
||
double[] rowArray = new double[columns];
|
||
for (int j = 0; j < columns; j++)
|
||
{
|
||
rowArray[j] = matrix[i, j];
|
||
}
|
||
|
||
list.Add(rowArray);
|
||
}
|
||
|
||
return list;
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 将 坐标数组转为 矩阵
|
||
/// </summary>
|
||
/// <param name="array">坐标数组</param>
|
||
/// <param name="cols">标准分列数</param>
|
||
public static double[,][] Array2Matrix(double[][] array, int cols)
|
||
{
|
||
// 总的行数
|
||
var rows = array.Length / cols;
|
||
|
||
var matrix = new double[rows, cols][];
|
||
|
||
for (var i = 0; i < rows; i++)
|
||
{
|
||
for (var j = 0; j < cols; j++)
|
||
{
|
||
matrix[i, j] = array[i * cols + j];
|
||
}
|
||
}
|
||
|
||
return matrix;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从矩阵的中心位置将其一分为二
|
||
/// </summary>
|
||
/// <param name="array"></param>
|
||
/// <returns></returns>
|
||
public static (double[,][], double[,][]) SplitMatrixByHalf(double[,][] array)
|
||
{
|
||
var rows = array.GetLength(0);
|
||
var cols = array.GetLength(1);
|
||
|
||
var middle = cols / 2;
|
||
|
||
var part1 = new double[rows, middle][];
|
||
var part2 = new double[rows, cols - middle][];
|
||
|
||
for (var i = 0; i < rows; i++)
|
||
{
|
||
for (var j = 0; j < cols; j++)
|
||
{
|
||
if (j < middle)
|
||
part1[i, j] = array[i, j];
|
||
else
|
||
part2[i, j - middle] = array[i, j];
|
||
}
|
||
}
|
||
|
||
return (part1, part2);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将矩阵数组转为弓字形数组
|
||
/// </summary>
|
||
/// <param name="matrix"></param>
|
||
/// <returns></returns>
|
||
public static double[][] ConvertMatrix2BowShapeArray(double[,][] matrix)
|
||
{
|
||
var rows = matrix.GetLength(0);
|
||
var cols = matrix.GetLength(1);
|
||
var result = new double[rows * cols][];
|
||
var index = 0;
|
||
|
||
for (var d = 0; d < rows + cols - 1; d++)
|
||
{
|
||
if (d % 2 == 0) // 从左下角到右上角
|
||
{
|
||
for (var i = Math.Min(d, rows - 1); i >= Math.Max(0, d - cols + 1); i--)
|
||
{
|
||
result[index++] = matrix[i, d - i];
|
||
}
|
||
}
|
||
else // 从右上角到左下角
|
||
{
|
||
for (var i = Math.Max(0, d - cols + 1); i <= Math.Min(d, rows - 1); i++)
|
||
{
|
||
result[index++] = matrix[i, d - i];
|
||
}
|
||
}
|
||
}
|
||
|
||
return result;
|
||
}
|
||
} |