logo标签列表

如何通过3个点计算一个在3D空间中的循环弧，并将其参数化为0到1。

c#mathrobotics
5

如何在三维空间内通过三个点A、B、C来计算一条弧线。从A到C经过B（顺序已解决）。

大多数机器人臂都有这种运动命令。我需要模拟它，并对其应用不同的速度动力学，因此需要一个0..1参数，将位置从A移动到C。

编辑：

我拥有圆弧的半径和中心，但如果我知道起始角度和结束角度，我如何将圆弧在三维空间中参数化呢？

编辑2：

已接近答案。如果我在圆所在平面上有两个垂直的单位长度向量v1和v2，那么我可以像这样进行参数化：x（t）= c + r * cos（t）* v1 + r * sin（t）* v2

所以我取v1 = a-c，现在我只需要找到v2了。有什么想法吗？

2
酷，你目前有什么进展？ - RhysW
“arc”指的是圆弧的一部分，还是仅指平滑路径？您是否了解点的相对位置（例如，距离|AC|是否大于|AB| + |BC|？） - Justin
1
计算圆上给定的三个点的参数几乎是微不足道的，谷歌肯定会为你提供相应的算法。既然你知道了所感兴趣的弧的端点的位置，那么你就拥有了所有需要知道的信息。 - High Performance Mark
好的，中心和半径没有问题，但是如何参数化弧呢？我已经有了目标角度，所以我可以从0..angle进行插值。但是在三维空间中，圆是如何参数化的呢？ - thalm
我终于找到了解决方案，请查看我的答案。 - thalm
显示剩余5条评论
3个回答
3

Martin Doms最近写了一篇关于样条曲线和贝塞尔曲线的博客文章，您可能会觉得有用。

他的帖子的一部分描述了如何通过三个控制点P0、P1和P2来获得一个二维曲线。该曲线由值从0到1变化的参数t参数化:

F(t) = (1-t)2 P0 + 2t (1-t) P1 + t2 P2

思考一下，你很可能可以将其调整为三维。当然，贝塞尔曲线不一定经过控制点。如果这是一个不能接受的问题，那么这种方法可能行不通。

顺带一提，Jason Davies制作了一个不错的曲线插值动画。

谢谢你的回答，我也考虑过贝塞尔曲线和样条曲线。你提到的方程在这里不适用，因为它是一个贝塞尔曲线，而且它不经过控制点。然而，我认为样条曲线可以。我需要再看一下它们。我想这绝对是正确的方向... - thalm
@thalm：是的，样条曲线会更好。你看过http://scaledinnovation.com/analytics/splines/aboutSplines.html吗？ - Nate Kohl
我在贝塞尔曲线或样条曲线中找不到解决方案，但可以看看我的答案，其中包含几何解决方案。 - thalm
2

重新回到这个问题，它非常棘手。代码尽可能地简短，但仍比我想象的要多得多。

您可以创建该类的实例，并使用正确顺序的3个位置调用SolveArc方法来设置该类。然后，Arc方法将以线性速度在0..1上给出圆弧上的位置。如果您找到更短的解决方案，请告诉我。

class ArcSolver
{
    public Vector3D Center { get; private set; }

    public double Radius { get; private set; }

    public double Angle { get; private set; }

    Vector3D FDirP1, FDirP2;

    //get arc position at t [0..1]
    public Vector3D Arc(double t)
    {
        var x = t*Angle;
        return Center + Radius * Math.Cos(x) * FDirP1 + Radius * Math.Sin(x) * FDirP2;
    }

    //Set the points, the arc will go from P1 to P3 though P2.
    public bool SolveArc(Vector3D P1, Vector3D P2, Vector3D P3)
    {
        //to read this code you need to know that the Vector3D struct has
        //many overloaded operators: 
        //~ normalize
        //| dot product
        //& cross product, left handed
        //! length

        Vector3D O = (P2 + P3) / 2;
        Vector3D C = (P1 + P3) / 2;
        Vector3D X = (P2 - P1) / -2;

        Vector3D N = (P3 - P1).CrossRH(P2 - P1);
        Vector3D D = ~N.CrossRH(P2 - O);
        Vector3D V = ~(P1 - C);

        double check = D|V;
        Angle = Math.PI;
        var exist = false;

        if (check != 0)
        {
            double t = (X|V) / check;
            Center = O + D*t;
            Radius = !(Center - P1);
            Vector3D V1 = ~(P1 - Center);

            //vector from center to P1
            FDirP1 = V1;
            Vector3D V2 = ~(P3 - Center);
            Angle = Math.Acos(V1|V2);

            if (Angle != 0)
            {
                exist = true;
                V1 = P2-P1;
                V2 = P2-P3;
                if ((V1|V2) > 0)
                {
                    Angle = Math.PI * 2 - Angle;
                }
            }
        }

        //vector from center to P2
        FDirP2 = ~(-N.CrossRH(P1 - Center));
        return exist;
    }
}
1

因为代码是用 Mathematica 而不是 C#，所以这个答案是故事的一部分，但是所有的数学（或许有一个小例外）都应该相对容易地转换到任何语言。

所提出的基本方法是：

  1. 将三个点（A、B、C）投影到这些点所在的平面上。它的法线应为 AB x BC。这将问题从三维降为二维。
  2. 使用您喜欢的技术找到通过三个投影点的圆的中心。
  3. 将圆的中心反投影回三维空间。
  4. 使用适当的球形插值策略（示例中使用了 slerp，但我认为最好使用四元数）。

唯一需要注意的是，您需要确定旋转的方向。我相信有更聪明的方法，但是只有两种选择，拒绝测试就足够了。我使用了 reduce，但是您可能需要做一些稍微不同的事情来在 C# 中实现。

不能保证这是最稳定或最健壮的方法，也不能保证没有遗漏任何边角情况。

(* Perpendicular vector in 2 dimensions *)
Perp2d[v_] := {-v[[2]], v[[1]]};

(* Spherical linear interpolation. From wikipedia \
http://en.wikipedia.org/wiki/Slerp *)
slerp[p0_, p1_, t_, rev_] :=
  Module[{\[CapitalOmega], v},
   \[CapitalOmega] = ArcCos[Dot[p0, p1]];
   \[CapitalOmega] = 
    If[rev == 0, 2 Pi - \[CapitalOmega], \[CapitalOmega]];
   v = (Sin[(1 - t) \[CapitalOmega]]/
        Sin[\[CapitalOmega]]) p0 + (Sin[t \[CapitalOmega]]/
        Sin[\[CapitalOmega]]) p1;
   Return[v]
   ];

(* Based on the expressions from mathworld \
http://mathworld.wolfram.com/Line-LineIntersection.html *)
IntersectionLineLine[{x1_, y1_}, {x2_, y2_}, {x3_, y3_}, {x4_, y4_}] :=
  Module[{x, y, A, B, C},
  A = Det[{{x1, y1}, {x2, y2}}];
  B = Det[{{x3, y3}, {x4, y4}}];
  C = Det[{{x1 - x2, y1 - y2}, {x3 - x4, y3 - y4}}];
  x = Det[{{A, x1 - x2}, {B, x3 - x4}}]/C;
  y = Det[{{A, y1 - y2}, {B, y3 - y4}}]/C;
  Return[{x, y}]
  ]

(* Based on Paul Bourke's Notes \
http://local.wasp.uwa.edu.au/~pbourke/geometry/circlefrom3/ *)
CircleFromThreePoints2D[v1_, v2_, v3_] :=
 Module[{v12, v23, mid12, mid23, v12perp, v23perp, c, r},
  v12 = v2 - v1;
  v23 = v3 - v2;
  mid12 = Mean[{v1, v2}];
  mid23 = Mean[{v2, v3}];
  c = IntersectionLineLine[
    mid12, mid12 + Perp2d[v12],
    mid23, mid23 + Perp2d[v23]
    ];
  r = Norm[c - v1];
  Assert[r == Norm[c - v2]];
  Assert[r == Norm[c - v3]];
  Return[{c, r}]
  ]

(* Projection from 3d to 2d *)
CircleFromThreePoints3D[v1_, v2_, v3_] :=
 Module[{v12, v23, vnorm, b1, b2, va, vb, vc, xc, xr, yc, yr},
  v12 = v2 - v1;
  v23 = v3 - v2;
  vnorm = Cross[v12, v23];
  b1 = Normalize[v12];
  b2 = Normalize[Cross[v12, vnorm]];
  va = {0, 0};
  vb = {Dot[v2, b1], Dot[v2, b2]};
  vc = {Dot[v3, b1], Dot[v3, b2]};
  {xc, xr} = CircleFromThreePoints2D[va, vb, vc];
  yc = xc[[1]] b1 + xc[[2]] b2;
  yr = Norm[v1 - yc];
  Return[{yc, yr, b1, b2}]
  ]

v1 = {0, 0, 0};
v2 = {5, 3, 7};
v3 = {6, 4, 2};

(* calculate the center of the circle, radius, and basis vectors b1 \
and b2 *)
{yc, yr, b1, b2} = CircleFromThreePoints3D[v1, v2, v3];

(* calculate the path of motion, given an arbitrary direction *)
path = Function[{t, d}, 
   yc + yr slerp[(v1 - yc)/yr, (v3 - yc)/yr, t, d]];

(* correct the direction of rotation if necessary *)
dirn = If[
  TrueQ[Reduce[{path[t, 1] == v2, t >= 0 && t <= 1}, t] == False], 0, 
  1]

(* Plot Results *)
gr1 = ParametricPlot3D[path[t, dirn], {t, 0.0, 1.0}];
gr2 = ParametricPlot3D[Circle3d[b1, b2, yc, yr][t], {t, 0, 2 Pi}];
Show[
 gr1,
 Graphics3D[Line[{v1, v1 + b1}]],
 Graphics3D[Line[{v1, v1 + b2}]],
 Graphics3D[Sphere[v1, 0.1]],
 Graphics3D[Sphere[v2, 0.1]],
 Graphics3D[{Green, Sphere[v3, 0.1]}],
 Graphics3D[Sphere[yc, 0.2]],
 PlotRange -> Transpose[{yc - 1.2 yr, yc + 1.2 yr}]
 ]

看起来大致如下：

Solution Image

非常感谢，您的代码帮助我很多，在我的答案中找到了解决方案。 - thalm
网页内容由stack overflow 提供, 点击上面的
可以查看英文原文，
原文链接