How to calculate a cyclic arc through 3 points and parameterize it 0..1 in 3d

Question

How can I calculate an arc through 3 points A, B, C in 3d. from A to C passing B (order is taken).

Most robots have such a move command. I need to simulate it and apply different speed dynamics to it, and therefore the parameter 0..1 is needed, which moves the position from A to C.

EDIT:

I have a radius and an arc center, but how can I parameterize a circle in 3d if I know the angle of the beginning and the end?

EDIT2:

is approaching. if I have perpendicular vectors v1 and v2 per unit length on the plane in which the circle lies, I can do the parameterization as: x (t) = c + r * cos (t) * v1 + r * sin (t) * v2

so i take v1 = ac and i only need to find v2. any ideas?

Answer 1

Returned to this, and it was quite difficult. The code is as short as possible, but still much more than I thought.

You can instantiate this class and call the SolveArc method with three positions (in the correct order) to configure the class. Then the Arc method will give you positions along the arc of a circle from 0..1 at linear speed. If you find a shorter solution, let me know.

 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; } } 

Answer 2

Martin Doms recently wrote a blog post about Bezier splines and curves that might be useful.

Part of his message describes how to get a 2D curve defined by three control points P ₀ , P ₁ and P _2. The curve is parameterized by the value of t , which ranges from 0 to 1:

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

It seems likely that you could adapt to 3D with a little thought. (Of course, bezier curves do not necessarily pass through breakpoints. This may not work if it breaks the deal for you.)

Aside, Jason Davis put together a nice animation of curve interpolation .

Answer 3

So, this answer is part of the story, given that the code is in Mathematica, and not in C #, but, of course, all mathematical data (perhaps one small exception) should be relatively easy to translate into any language.

The main approach presented:

• Project three points ( A , B , C ) onto the plane where these points are. It should have normal AB x BC . This reduces the problem from three dimensions to two.
• Use your favorite method to find the center of a circle that passes through three projected points.
• Do not project the center of the circle back into three dimensions.
• Use the appropriate spherical interpolation strategy (slerp is used in the sample, but I think it would be better to use quaternions).

One caveat is that you need to decide which direction you are heading in, I'm sure there are more reasonable ways, but with only two alternatives, testing rejection is sufficient. I use reduce , but you probably need to do something a little different in order to do this in C #

There is no guarantee that this is the most numerically stable or reliable way to do this, or that there are any corner cases that were missed.

 (* 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 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}] ] 

Which looks something like this: