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

c#, math, robotics

Solution

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

Problem

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?

Original source