C# 如何通过3个点计算循环圆弧，并在三维中将其参数化为0..1

如何计算三维中通过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=a-c，现在只需要找到v2。有什么想法吗？

马丁·多姆斯最近写道，你可能会觉得有用

他的部分文章描述了如何获得由三个控制点P0、P1和P2定义的二维曲线。曲线由范围为0到1的值

t

参数化：

F（t）=（1-t）2p0+2t（1-t）P1+t2p2

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

---

顺便说一句，Jason Davies总结了一下。

这个答案是故事的一部分，因为代码是用Mathematica而不是C#编写的，但是所有的数学（也许有一个小的例外）都应该相对容易翻译成任何语言

提出的基本方法是：

将三个点（A、B、C）投影到这些点所在的平面上。它应该有一个正常的ABxBC。这将问题从三维减少到二维

使用您最喜欢的技术查找通过三个投影点的圆心

将圆心反投影回三维

使用合适的球面插值策略（示例中使用了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}]
 ]

（*二维垂直向量*）
Perp2d[v_]：={-v[[2]]，v[[1]}；
（*球面线性插值。来自维基百科\
http://en.wikipedia.org/wiki/Slerp *)
slerp[p0，p1，t，rev]：=
模块[{\[CapitalOmega]，v}，
\[CapitalOmega]=ArcCos[Dot[p0，p1]]；
\[CapitalOmega]=
如果[rev==0，2 Pi-\[CapitalOmega]，\[CapitalOmega]；
v=（Sin[（1-t）\[CapitalOmega]]/
Sin[\[CapitalOmega]]p0+（Sin[t\[CapitalOmega]]/
Sin[\[CapitalOmega]]）p1；
返回[v]
];
（*基于mathworld的表达式\
http://mathworld.wolfram.com/Line-LineIntersection.html *)
相交线[{x1_，y1_}，{x2_，y2_}，{x3_，y3_}，{x4_，y4_}]：=
模[{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；
返回[{x，y}]
]
（*基于Paul Bourke的笔记）\
http://local.wasp.uwa.edu.au/~pburke/geometry/circlefrom3/*）
三点2d[v1_u，v2_，v3_u3]的圆圈：=
模块[{v12，v23，mid12，mid23，v12perp，v23perp，c，r}，
v12=v2-v1；
v23=v3-v2；
mid12=平均值[{v1，v2}]；
mid23=平均值[{v2，v3}]；
c=相交线[
mid12，mid12+Perp2d[v12]，
mid23，mid23+Perp2d[v23]
];
r=标准值[c-v1]；
断言[r==Norm[c-v2]]；
断言[r==Norm[c-v3]]；
返回[{c，r}]
]
（*从3d到2d的投影*）
三点3d[v1_u，v2_，v3_u3]的圆圈：=
模块[{v12，v23，vnorm，b1，b2，va，vb，vc，xc，xr，yc，yr}，
v12=v2-v1；
v23=v3-v2；
vnorm=交叉[v12，v23]；
b1=正常化[v12]；
b2=标准化[交叉[v12，vnorm]；
va={0，0}；
vb={Dot[v2，b1]，Dot[v2，b2]}；
vc={Dot[v3，b1]，Dot[v3，b2]}；
{xc，xr}=三点2d[va，vb，vc]的圆圈；
yc=xc[[1]]b1+xc[[2]]b2；
yr=标准值[v1-yc]；
返回[{yc，yr，b1，b2}]
]
v1={0,0,0}；
v2={5,3,7}；
v3={6,4,2}；
（*计算圆心、半径和基向量b1\
和b2*）
{yc，yr，b1，b2}=三点3d[v1，v2，v3]的圈；
（*计算给定任意方向的运动路径*）
路径=函数[{t，d}，
yc+年slerp[（v1-yc）/年，（v3-yc）/年，t，d]；
（*必要时纠正旋转方向*）
dirn=If[
TrueQ[Reduce[{path[t，1]==v2，t>=0&&t转置[{yc-1.2年，yc+1.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;
    }
}

---

酷，到目前为止你得到了什么？所谓“弧”，你是指一条，还是仅仅是一条平滑的路径？你知道点的相对位置吗（例如，距离是| AC |>AB |+BC |？）是的，一个圆的一部分。三点可以是任意的，所以圆弧几乎可以变成一个完整的圆。到目前为止，我们有很多的动作和动态，只是圆周运动消失了：计算PAR。