How to calc 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 care of).

Most robot arms have this kind of move command. I need to simulate it and apply different speed dynamics to it and need therefore a parameter 0..1 which moves a position from A to C.

EDIT:

what i have is radius and center of the arc, but how can i parameterize the circle in 3d if i know the start and end angle?

EDIT2:

getting closer. if i have two unit length perpendicular vectors v1 and v2 on the plane in which the circle lies, i can do a parameterization like: x(t) = c + r * cos(t) * v1 + r * sin(t) * v2

so i take v1 = a-c and i only need to find v2 now. any ideas?

Answer 1

Martin Doms recently wrote a blog entry about splines and bezier curves that you might find useful.

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

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

It seems likely that you could adapt that to 3D with a little thought. (Of course, bezier curves don't necessarily go through the control points. This may not work if that's a deal-breaker for you.)

As an aside, Jason Davies put together a nice little animation of curve interpolation.

Answer 2

Got back to this one and it was quite tricky. The code is as short as possible, but still much more than i thought.

You can create an instance of this class and call the SolveArc method with the 3 positions (in the right order) to set up the class. Then the Arc Method will give you the positions on the circular arc from 0..1 in linear velocity. If you find a shorter solution, please 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 3

So this answer is part of the story, given that the code is in Mathematica rather than C#, but certainly all of the maths (with perhaps one minor exception) should all be relatively easy to translate to any language.

The basic approach presented is to:

1. Project the three points (A, B, C) onto the plane that those points lie in. It should have a normal AB x BC. This reduces the problem from three dimensions to two.
2. Use your favourite technique for finding the center of the circle that passes through the three projected points.
3. Unproject the center of the circle back into three dimensions.
4. Use an appropriate spherical interpolation strategy (slerp is used in the sample, but I believe it would have been better to use quaternions).

The one caveat is that you need to work out which direction you are rotating in, I'm sure there are smarter ways, but with only two alternatives, rejection testing is sufficient. I'm using reduce, but you'd probably need to do something slightly different to do this in C#

No guarantees that this is the most numerically stable or robust way to do this, or that there are any corner cases that have been 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'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}]
 ]

Which looks something like this: