[Math] 3 Points in 3D Space to find the center of an Arc or Circle

3dcirclesgeometrylinear algebramatrices

Background: I'm a Robotics Engineer and I am trying to develop a more flexible, modular, and robust program for our welding robots, which will minimize teaching time for new robots and also minimize the amount of damage team members can do if the mess up reprogramming a path. The robots I am working on need to weld along a changing curved path in 3D space. I have found several helpful answers on here but I am having a little trouble tying them together.

Problem: For the current portion of the program, I need to take three points in 3D space, and calculate the center of curvature. I plan to use this to parse up a path into regular segments, and then describe those regular segments as either circular arcs or straight lines in the same 3D space. For now, I need to find the center of the arc through the three points. I have found the equations to do this in 2D space, and they are pretty simple. However, I was never very good with matrix algebra and that is a BIG part of moving into the 3D space. Any hep on this project would be appreciated!

Best Answer

I suggest turning this into a 2D problem and then find the circle from three points on the plane.

NOTE: Below the $\times$ is vector cross product and $\cdot$ the vector dot product. Also $^\top$ is a matrix transpose (switch rows with columns). Finally $\| \boldsymbol{x} \|$ returns the magnitude of the vector $\boldsymbol{x}$.

Start by finding the normal vector to the plane defined by the three points

$$ \boldsymbol{n} = \mathrm{unitvector}( \boldsymbol{A} \times \boldsymbol{B} + \boldsymbol{B} \times \boldsymbol{C} + \boldsymbol{C} \times \boldsymbol{A}) \tag{1}$$

The find the arbitrary mutually orthogonal directions

$$ \boldsymbol{u} = (\boldsymbol{C}-\boldsymbol{A})\times \boldsymbol{n} \tag{2}$$ $$ \boldsymbol{v} = \boldsymbol{n} \times \boldsymbol{u} \tag{3}$$

and construct a 3×2 rotation matrix from the three direction vectors as columns.

$$ \mathbf{R} = \left[ \boldsymbol{u} \; \boldsymbol{v} \right] \tag{4}$$

You also need the distance of the plane ABC to the origin

$$ d = \boldsymbol{n} \cdot \boldsymbol{A} \tag{5}$$

Now convert the problem into a 2D problem with

$$ \begin{aligned} \boldsymbol{a} = \pmatrix{a_x\\a_y} & = \mathbf{R}^\top \boldsymbol{A}\\ \boldsymbol{b} = \pmatrix{b_x\\b_y} & = \mathbf{R}^\top \boldsymbol{B}\\ \boldsymbol{c} = \pmatrix{b_x\\b_y} & = \mathbf{R}^\top \boldsymbol{B} \end{aligned} \tag{6}$$

Now solve the 2D problem by finding the center point $\boldsymbol{q}=\pmatrix{x\\y}$ using a 2×2 system of equations

$$ \begin{bmatrix} b_x-a_x & b_y -a_y \\ c_x -a_x & c_y - a_y \end{bmatrix} \pmatrix{x\\y} = \pmatrix{ \frac{ (b_x^2+b_y^2)-(a_x^2+a_y^2)}{2} \\ \frac{ (c_x^2+c_y^2)-(a_x^2+a_y^2)}{2} } \tag{7} $$

Lastly construct the 3D point for the circle center $\boldsymbol{Q}$

$$ \boldsymbol{Q} = \boldsymbol{n}\, d + \mathbf{R} \pmatrix{x\\y} \tag{8}$$

If the radius is needed just take the distance from the center to any point

$$ \mathrm{radius} = \| \boldsymbol{A}-\boldsymbol{Q} \| \tag{9} $$

You might also need the sweep angle which you get from the angle between the vectors $\boldsymbol{C}-\boldsymbol{Q}$ and $\boldsymbol{A}-\boldsymbol{Q}$

$$ \theta = \cos^{-1} \left( \frac{ (\boldsymbol{C}-\boldsymbol{Q}) \cdot ( \boldsymbol{A}-\boldsymbol{Q}) }{ \| \boldsymbol{C}-\boldsymbol{Q}\| \,\|\boldsymbol{A}-\boldsymbol{Q} \| } \right) \tag{10} $$


Example

Points:

$$\begin{aligned} \boldsymbol{A} &= \pmatrix{1\\0\\0} & \boldsymbol{B} & = \pmatrix{3\\1\\0} & \boldsymbol{C} &= \pmatrix{2 \\0 \\ -1} \end{aligned} $$

Normal:

$$ \boldsymbol{n} = \pmatrix{ -\tfrac{\sqrt{6}}{6} \\ \tfrac{\sqrt{6}}{3} \\ -\tfrac{\sqrt{6}}{6} } $$

Distance from Origin

$$ d = -\tfrac{\sqrt 6}{6} $$

Rotation:

$$ \mathbf{R} = \begin{bmatrix} \tfrac{\sqrt 3}{3} & \tfrac{\sqrt 2}{2} \\ \tfrac{\sqrt 3}{3} & 0 \\ \tfrac{\sqrt 3}{3} & -\tfrac{\sqrt 2}{2} \end{bmatrix} $$

2D Points:

$$ \begin{aligned} \boldsymbol{a} & = \pmatrix{ \tfrac{\sqrt 3}{3} \\ \tfrac{\sqrt 2}{2} } & \boldsymbol{b} & = \pmatrix{ \tfrac{4 \sqrt 3}{3} \\ \tfrac{3 \sqrt 2}{2} } & \boldsymbol{c} & = \pmatrix{ \tfrac{\sqrt 3}{3} \\ \tfrac{3 \sqrt 2}{2} } \end{aligned} $$

System of equations:

$$\left. \begin{bmatrix} \sqrt{3} & \sqrt{2} \\ 0 & \sqrt{2} \end{bmatrix} \pmatrix{x \\ y} = \pmatrix{\tfrac{9}{2} \\ 2 } \;\right\} \; \pmatrix{x = \tfrac{5 \sqrt 3}{6}\\y = \sqrt 2} $$

Circle Center:

$$ \boldsymbol{Q} = \left( -\tfrac{\sqrt 6}{6}\right) \pmatrix{ -\tfrac{\sqrt{6}}{6} \\ \tfrac{\sqrt{6}}{3} \\ -\tfrac{\sqrt{6}}{6} } + \begin{bmatrix} \tfrac{\sqrt 3}{3} & \tfrac{\sqrt 2}{2} \\ \tfrac{\sqrt 3}{3} & 0 \\ \tfrac{\sqrt 3}{3} & -\tfrac{\sqrt 2}{2} \end{bmatrix} \pmatrix{ \tfrac{5 \sqrt 3}{6}\\ \sqrt 2} = \pmatrix{2 \\ \tfrac{1}{2} \\ 0} $$

Prove that points A, B, and C are equidistant to Q

$$ \mathrm{radius} = \| \boldsymbol{A}-\boldsymbol{Q} \| = \| \boldsymbol{B}-\boldsymbol{Q} \| = \| \boldsymbol{C}-\boldsymbol{Q} \| = \tfrac{\sqrt 5}{2} \;\;\checkmark $$

PS. I am also a fellow roboticist. You do need a basic understanding of linear algebra operations as well as some vector calculus to understand the how and why of things in robotics.