c# - 如何通过 3 个点计算循环弧并在 3d 中将其参数化为 0..1

Question

我如何通过 3d 中的 3 个点 A、B、C 计算弧线。从 A 到 C 传递 B（处理顺序）。

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

编辑：

我所拥有的是圆弧的半径和中心，但是如果我知道开始和结束角度，我如何在 3d 中参数化圆？

编辑2：

越来越近。如果我在圆所在的平面上有两个单位长度的垂直向量 v1 和 v2，我可以进行如下参数化：x (t) = c + r * cos(t) * v1 + r * sin(t) * v2

所以我取v1 = ac，我现在只需要找到v2。有任何想法吗？

score 2 · Accepted Answer

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

他的部分文章描述了如何获得由三个控制点 P ₀、 P ₁和 P ₂定义的二维曲线。t该曲线由一个范围从 0 到 1的值参数化：

F(t) = (1-t) ² P ₀ + 2t (1-t) P ₁ + t ² P ₂

您似乎可以稍加思考就可以将其适应 3D。（当然，贝塞尔曲线不一定通过控制点。如果这对你来说是一个交易破坏者，这可能不起作用。）

顺便说一句，Jason Davies 制作了一个不错的曲线插值小动画。

score 2 · Accepted Answer

回到这个，这很棘手。代码尽可能短，但仍然比我想象的要多。

您可以创建此类的一个实例并调用SolveArc具有 3 个位置的方法（以正确的顺序）来设置该类。然后，该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;
    }
}

score 1 · Accepted Answer

所以这个答案是故事的一部分，因为代码是在 Mathematica 而不是 C# 中，但当然所有的数学（可能有一个小例外）都应该相对容易翻译成任何语言。

提出的基本方法是：

将三个点（A、B、C）投影到这些点所在的平面上。它应该有一个法线AB x BC。这将问题从三个维度减少到了两个维度。
使用您最喜欢的技术来找到通过三个投影点的圆心。
将圆心取消投影回三个维度。
使用适当的球面插值策略（示例中使用了 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}]
 ]

看起来像这样：

解决方案图片

c# - 如何通过 3 个点计算循环弧并在 3d 中将其参数化为 0..1

3 回答 3

Related

Reference