场景：控制库卡六轴机械臂等在空间中运动，要求根据配置的3个点位（左上，左下，右上）进行计算，算出这3个点在空间中构成的矩形，计算出在这个矩形中（即不包括边界点位），等距离间隔的所有点位坐标。局部坐标系转换成全局坐标系可以处理空间中任意方向的矩形。无需考虑平面定义xoy,zoy,xoz。
理论依据：
基于向量的线性组合和坐标变换原理。假设入参是三个点 P₁(x₁,y₁,z₁), P₂(x₂,y₂,z₂), P₃(x₃,y₃,z₃)，最初判断这三个点是否允许形成一个矩形。向量 P₁P₂ 和 P₁P₃需垂直，即点积为0。然后计算出第四个点P₄ = P₁ + (P₂ - P₁) + (P₃ - P₁) = (x₂ + x₃ - x₁, y₂ + y₃ - y₁, z₂ + z₃ - z₁)。结果建立全局坐标系，生成所有点位。
C#版代码实现：
public class Point
{
public double X { get; set; }
public double Y { get; set; }
public double Z { get; set; }
public Point(double x, double y, double z)
{
X = x;
Y = y;
Z = z;
}
public override string ToString()
{
return $"({X}, {Y}, {Z})";
}
}
public class GetCoordinates3D
{
public static List<Point> getPoints(Point p1, Point p2, Point p3, double dist )
{
/* u,v计算向量*/
Vector3D u = new Vector3D(p2.X - p1.X, p2.Y - p1.Y, p2.Z - p1.Z);
Vector3D v = new Vector3D(p3.X - p1.X, p3.Y - p1.Y, p3.Z - p1.Z);
if (Math.Abs(Vector3D.Dot(u, v)) > 0.0001)
{
throw new ArgumentException("无法构成矩形，请重新输入三个点位参数");
}
double length = u.Magnitude();//长度
double width = v.Magnitude();//宽
// 计算点范围
List<double> xSteps = new List<double>();
List<double> ySteps = new List<double>();
for (double x = dist; x < length; x += dist)
{
xSteps.Add(x);
}
for (double y = dist; y < width; y += dist)
{
ySteps.Add(y);
}
List<Point> points = new List<Point>();//点位集合定义
// 单位向量
Vector3D uUnit = u.Normalize();
Vector3D vUnit = v.Normalize();
foreach (double x in xSteps)
{
foreach (double y in ySteps)
{
// 转换到全局坐标系
Point point = new Point(
p1.X + x * uUnit.X + y * vUnit.X,
p1.Y + x * uUnit.Y + y * vUnit.Y,
p1.Z + x * uUnit.Z + y * vUnit.Z
);
points.Add(point);
}
}
return points;
}
}
public class Vector3D
{
public double X { get; set; }
public double Y { get; set; }
public double Z { get; set; }
public Vector3D(double x, double y, double z)
{
X = x;
Y = y;
Z = z;
}
public double Magnitude()
{
return Math.Sqrt(X * X + Y * Y + Z * Z);
}
public Vector3D Normalize()
{
double mag = Magnitude();
if (mag == 0)
return new Vector3D(0, 0, 0);
return new Vector3D(X / mag, Y / mag, Z / mag);
}
public static double Dot(Vector3D a, Vector3D b)
{
return (a.X * b.X + a.Y * b.Y + a.Z * b.Z);
}
}
class Program
{
static void Main(string[] args)
{
try
{
/*测试参数XYZ,点位间距10*/
Point p1 = new Point(0, 0, 0);
Point p2 = new Point(100, 0, 0);
Point p3 = new Point(0, 50, 0);
List<Point> points = GetCoordinates3D.getPoints(p1, p2, p3, 10);
Console.WriteLine($"生成 {points.Count} 点位");
Console.WriteLine("以下是点坐标：");
for (int i = 0; i < points.Count; i++)
{
Console.WriteLine($"点{i + 1}: {points[i]}");
}
}
catch (Exception ex)
{
Console.WriteLine($"错误: {ex.Message}");
}
}
}