3Dで3点A、B、Cを通る円弧を計算するにはどうすればよいですか? A から C へ B を通過します (順序が処理されます)。
ほとんどのロボット アームには、この種の移動コマンドがあります。それをシミュレートし、異なる速度ダイナミクスを適用する必要があるため、位置を A から C に移動するパラメーター 0..1 が必要です。
編集：
私が持っているのは円弧の半径と中心ですが、開始角度と終了角度がわかっている場合、3D で円をパラメータ化するにはどうすればよいですか?
EDIT2：
近くなってる。円が存在する平面上に 2 つの単位長の垂直ベクトル v1 と v2 がある場合、次のようなパラメーター化を行うことができます: x (t) = c + r * cos(t) * v1 + r * sin(t) * v2
だから私は v1 = ac を取り、今は v2 を見つけるだけです。何か案は？
これに戻って、かなりトリッキーでした。コードは可能な限り短いですが、それでも思ったよりもはるかに短いです。
このクラスのインスタンスを作成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;
}
}
Martin Doms は最近、役に立つと思われる スプラインとベジェ曲線に関するブログ エントリを書きました。
彼の投稿の一部では、3 つの制御点 P 0、P 1、および P 2によって定義される 2D 曲線を取得する方法について説明しています。t曲線は、0 から 1 の範囲の値によってパラメーター化されます。
F(t) = (1-t) 2 P 0 + 2t (1-t) P 1 + t 2 P 2
少し考えれば、それを 3D に適応させることができそうです。(もちろん、ベジエ曲線は必ずしもコントロール ポイントを通過するとは限りません。それが問題になる場合、これは機能しない可能性があります。)
余談ですが、Jason Daviesが曲線補間の素敵な小さなアニメーションをまとめました。
したがって、コードが C# ではなく Mathematica であることを考えると、この回答は話の一部ですが、確かにすべての数学 (おそらく 1 つの小さな例外を除く) はすべて、どの言語にも比較的簡単に翻訳できるはずです。
提示される基本的なアプローチは次のとおりです。
1 つの注意点は、どちらの方向に回転しているかを理解する必要があるということです。よりスマートな方法があると確信していますが、2 つの選択肢しかないため、拒否テストで十分です。私は を使用して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}]
]
これは次のようになります。
