Calculate 3D arc from three vectors and a given radius

Question

How can i add an arc where two 3D lines meet?

Known: Radius, P1, P2, P3.

what i need: Pa (arc start point3D), Pb (arc end Point3D), Pc (arc center point3D),

        double radius = 20;
        Vector3D P1 = new Vector3D(341.21, 227.208, 193.38);
        Vector3D P2 = new Vector3D(360.78, 85.34, 245.723);
        Vector3D P3 = new Vector3D(614.64, 85.34, 150.80);

        Vector3D P2P1 = new Vector3D(); 
        P2P1 = P1 - P2;


        Vector3D P2P3 = new Vector3D();
        P2P3 = P3 - P2;

I have found this answer on math.stackexchange, but i don't know all math symbols https://math.stackexchange.com/questions/2343931/how-to-calculate-3d-arc-between-two-lines

Update#1:

Thanks to @Spektre i was able to solve this problem like this

Had to use a new CAD model, lost the old one.

using System;
using System.Collections.ObjectModel;
using System.Windows;
using System.Windows.Media.Media3D;

namespace WpfApp1
{
    public partial class MainWindow : Window
    {
        public MainWindow()
        {
            InitializeComponent();

            double r = 0.5;
            Vector3D P1 = new Vector3D(1.04004, 1.37919, 1.31332);
            Vector3D P2 = new Vector3D(2.78928, 2.34881, 1.31332);
            Vector3D P3 = new Vector3D(2.66790, 2.56780, 0.34518);
            Vector3D n = Vector3D.CrossProduct(P2 - P1, P3 - P1);
            n.Normalize();

            Vector3D d12 = new Vector3D();
            Vector3D d23 = new Vector3D();

            Vector3D ttt = Vector3D.CrossProduct(P2 - P1, n);
            Vector3D jjj = Vector3D.CrossProduct(P3 - P2, n);
            ttt.Normalize();
            jjj.Normalize();
            d12 = +- r * ttt;
            d23 = +- r * jjj;

            Vector3D A12 = new Vector3D();
            Vector3D B12 = new Vector3D();
            Vector3D A23 = new Vector3D();
            Vector3D B23 = new Vector3D();

            // Result 
            // One line displaced by radius
            A12 = P1 + d12;
            B12 = P2 + d12;
            // One line displaced by radius
            B23 = P2 + d23;
            A23 = P3 + d23;

            line3D line1 = new line3D( A12,  B12 ) ;
            line3D line2 = new line3D( A23,  B23 ) ;
            line3D ArcCenter = closest( line1, line2 );

            // CAD reference model,  Arc Center point3D   2,29128 2,21590 0,82925
            // Debug result 
            //center.dp { 0; 0; 0}
            //center.p0 { 2,29127973078106; 2,21590093975731; 0,829246317165185}
            //center.p1 { 2,29127973078106; 2,21590093975731; 0,829246317165185}


            Vector3D C = new Vector3D(ArcCenter.p0.X, ArcCenter.p0.Y, ArcCenter.p0.Z);

            Vector3D Pa = new Vector3D();
            Pa = C - d12;

            Vector3D Pb = new Vector3D();
            Pb = C - d23;

            Vector3D u = new Vector3D();
            u = Pa - C;

            Vector3D v = new Vector3D();
            v = Vector3D.CrossProduct(u, n);

            Vector3D d = new Vector3D();
            d = -d23 / (r * r);

            double ang; 
            ang = Math.Atan2(Vector3D.DotProduct(d, v), Vector3D.DotProduct(d, u));
            double a;            
            double da;
            int i;
            Collection<Vector3D> Vector3DList = new Collection<Vector3D>();

            for (a = 0.0, da = ang * 0.1, i = 0; i <= 10; a += da, i++)
            {
                Vector3DList.Add(C + u * Math.Cos(a) + v * Math.Sin(a));

            }
        }



        public line3D closest(line3D l0, line3D l1)
        {
            Vector3D u = l0.p1 - l0.p0;
            Vector3D v = l1.p1 - l1.p0;
            Vector3D w = l0.p0 - l1.p0;
            double a = Vector3D.DotProduct(u, u);       // always >= 0
            double b = Vector3D.DotProduct(u, v);
            double c = Vector3D.DotProduct(v, v);       // always >= 0
            double d = Vector3D.DotProduct(u, w);
            double e = Vector3D.DotProduct(v, w);
            double D = a * c - b * b;                   // always >= 0
            double t0;
            double t1;

            // compute the line3D parameters of the two closest points
            t0 = (b * e - c * d) / D;
            t1 = (a * e - b * d) / D;
            line3D r = new line3D(l0.p0 + l0.dp * t0, l1.p0 + l1.dp * t1);
            return r;
        }
        public class line3D
        {
            // cfg
            public Vector3D p0 = new Vector3D();
            public Vector3D p1 = new Vector3D();

            // computed
            public double l = 0;
            public Vector3D dp = new Vector3D();

            public line3D(Vector3D _p0, Vector3D _p1)
            {
                p0 = _p0;
                p1 = _p1;
                compute();
            }
            void compute()
            {
                dp = p1 - p0;
                l = dp.Length;
            }
        }
    }
}

Spektre · Accepted Answer · 2022-03-06T18:16:43.940

0

Assuming the arc is circular (your image shows some curve not looking even as an Ellipse so its most likely just BEZIER)?

Anyway the arc will be in the same plane as P1,P2,P3 so you can use Basis vectors to convert the problem to 2D ... Or you can do it directly in 3D using vector math I see it like this:

displace your lines P1P2 and P2P3 by radius r inwards

So you need a normal vector of the plane P1P2P3 for example:
```
n = normalize(cross(P2-P1,P3-P1))
```
now just create displacement vectors (polarity depends on your winding rule)
```
d12 = +/- r*normalize(cross(P2-P1,n))
d23 = +/- r*normalize(cross(P3-P2,n))
```
and displace
```
A12 = P1 + d12
B12 = P2 + d12
A23 = P2 + d23
B23 = P3 + d23
```

compute intersection of displaced lines A12B12 and A23B23

This will give you the circle center C of your arc. Here is line line intersection computation taken from my C++ geometry engine:

 line3D closest(line3D l0,line3D l1)
     {
     vec3 u=l0.p1-l0.p0;
     vec3 v=l1.p1-l1.p0;
     vec3 w=l0.p0-l1.p0;
     float a=dot(u,u);       // always >= 0
     float b=dot(u,v);
     float c=dot(v,v);       // always >= 0
     float d=dot(u,w);
     float e=dot(v,w);
     float D=a*c-b*b;        // always >= 0
     float t0,t1;
     point3D p;
     line3D r,rr;
     int f;                  // check distance perpendicular to: 1: l0, 2: l1
     f=0; r.l=-1.0;
     // compute the line3D parameters of the two closest points
     if (D<acc_dot) f=3;    // the lines are almost parallel
     else{
         t0=(b*e-c*d)/D;
         t1=(a*e-b*d)/D;
         if (t0<0.0){ f|=1; t0=0.0; }
         if (t0>1.0){ f|=1; t0=1.0; }
         if (t1<0.0){ f|=2; t1=0.0; }
         if (t1>1.0){ f|=2; t1=1.0; }
         r=line3D(l0.p0+(l0.dp*t0),l1.p0+(l1.dp*t1));
         }
     // check perpendicular distance from each endpoint marked in f
     if (int(f&1))
         {
         t0=0.0;
         t1=divide(dot(l0.p0-l1.p0,l1.dp),l1.l*l1.l);
         if (t1<0.0) t1=0.0;
         if (t1>1.0) t1=1.0;
         rr=line3D(l0.p0+(l0.dp*t0),l1.p0+(l1.dp*t1));
         if ((r.l<0.0)||(r.l>rr.l)) r=rr;
         t0=1.0;
         t1=divide(dot(l0.p1-l1.p0,l1.dp),l1.l*l1.l);
         if (t1<0.0) t1=0.0;
         if (t1>1.0) t1=1.0;
         rr=line3D(l0.p0+(l0.dp*t0),l1.p0+(l1.dp*t1));
         if ((r.l<0.0)||(r.l>rr.l)) r=rr;
         }
     if (int(f&2))
         {
         t1=0.0;
         t0=divide(dot(l1.p0-l0.p0,l0.dp),l0.l*l0.l);
         if (t0<0.0) t0=0.0;
         if (t0>1.0) t0=1.0;
         rr=line3D(l0.p0+(l0.dp*t0),l1.p0+(l1.dp*t1));
         if ((r.l<0.0)||(r.l>rr.l)) r=rr;
         t1=1.0;
         t0=divide(dot(l1.p1-l0.p0,l0.dp),l0.l*l0.l);
         if (t0<0.0) t0=0.0;
         if (t0>1.0) t0=1.0;
         rr=line3D(l0.p0+(l0.dp*t0),l1.p0+(l1.dp*t1));
         if ((r.l<0.0)||(r.l>rr.l)) r=rr;
         }
     return r;
     }

Each line has endpoints p0,p1 and displacement/direction dp=p1-p0 and length l=|dp|. The function returns 2 closest points to each other one belonging to line l0 and the other to l1 so if they match (their distance is near zero or less) they are the intersection C we are looking for. If not the lines are either parallel or too short or far from each other (or you chose wrong polarity for one or both displacements)

compute arc endpoints
```
Pa = C - d12
Pb = C - d23
```
compute basis vectors u,v for your arc

for exmaple:
```
u = Pa - c
v = cross(u,n)
```

compute edge angles for you arc

As I chose u as start of the arc directly we need just the ending angle

d = -d23/(r*r)
ang = atan2(dot(d,v),dot(d,u))
// if (ang>M_PI) ang-=2.0*M_PI    // use smaller arc however usual atan2 is in <-PI,+PI> range so no need for this

render your arc

any point on your arc will be computed like this:
```
p(a) = C + u*cos(a) + v*sin(a)
a = <0 , ang>
```

In case you are not familiar with 3D vector math operations (dot,cross,..) you can find their equations and implementation in here (look for [Edit2]):

Understanding 4x4 homogenous transform matrices

edited Mar 06 '22 at 18:16

answered Mar 06 '22 at 08:42

Spektre

49,595
11
110
380

Great, I will give it a try:-) the arc is circular. – Laurits S Mar 06 '22 at 12:48
step 1."displace your lines" is now working, step. 2. compute intersection of displaced lines , line3D(l0.p0 + (l0.dp * t0), l1.p0 + (l1.dp * t1)); is this a method ? is line3D.p0 a point3D? – Laurits S Mar 06 '22 at 14:35
@LauritsS yes... if you look at the link to the geometry engine there is complete source so you can see what is inside ... you can use any line / line intersection you got at your disposal ... I shared mine just in case you do not have any. Also you can use axis/axis intersection too (that one is simpler a bit) – Spektre Mar 06 '22 at 18:17
Okay, Yes i see it, Thanks. – Laurits S Mar 06 '22 at 21:29
I got step 1>5 working now, its a amazing to see those numbers being exactly the same "CAD model/code result" :-)) 6.render your arc p(a) = C + u*cos(a) + v*sin(a) what is "a"? a = <0 , ang> i cannot convert this syntax, is there another way to write it ? – Laurits S Mar 09 '22 at 07:38
@LauritsS ... I do not know C# syntax... If the rendering is problem then show a piece of code that renders a curve and I will try to port it ... – Spektre Mar 09 '22 at 08:07
@LauritsS `p,C,u,v` are 3D vectors `p(a)` is math notation and means `p` is function of `a` , `a` is scalar angle in radians , `cos(a),sin(a)` are basic scalar goniometrics using radians and `a = <0 , ang>` means that you loop `a` from zero to `ang` like: `for (a=0.0,da=ang*0.01,i=0;i<=100;a+=da,i++)` What exactly is the problem ? `p(a)=C+ucos(a) + vsin(a)` should be `p=C+u*cos(a) + v*sin(a)` in code , maybe you have function for multiplying vector by constant instead of using `*` or your `sin,cos` need some prefix like `Math.sin()` or `Math::sin()` or just include/import math module/lib – Spektre Mar 09 '22 at 08:13
@LauritsS also some languages are using degrees instead of radians however ang is result of atan2 so it should be already in the correct units – Spektre Mar 09 '22 at 08:15
double ang; ang = Math.Atan2(Vector3D.DotProduct(d, v), Vector3D.DotProduct(d, u)); double a = 4; Vector3D p = C + u * Math.Cos(a) + v * Math.Sin(a); double da; int i; for (a = 0.0, da = ang * 0.01, i = 0; i <= 10; a += da, i++) { } – Laurits S Mar 09 '22 at 08:49
this syntax do not give me any error, does it look correct to you ? – Laurits S Mar 09 '22 at 08:49
@LauritsS syntax is ok but why `a=4?` but that is not important what is important the `p=C+...` should be inside your `for` loop along with either storing the point `p` to some array or passing it to rendering engine directly, also the angle change should match your `1/(number of points-1)` per curve so: `for (a = 0.0, da = ang * 0.1, i = 0; i <= 10; a += da, i++) { p = C + u * Math.Cos(a) + v * Math.Sin(a); plot_point(p); }` that should produce 11 points along the arc ordered from Pa to Pb (both are included) – Spektre Mar 09 '22 at 08:59
1

Okay, every thing is working now, WOW! `a = 4;` i thought maybe it was arc divisions. it looks like this now double ang; ang = Math.Atan2(Vector3D.DotProduct(d, v), Vector3D.DotProduct(d, u)); double a; double da; int i; Collection Vector3DList = new Collection(); for (a = 0.0, da = ang * 0.1, i = 0; i <= 10; a += da, i++) { Vector3DList.Add(C + u * Math.Cos(a) + v * Math.Sin(a)); } – Laurits S Mar 09 '22 at 09:48
@LauritsS I think you should add working code to your question for others looking for this in future so they do not need to port mine math to C# again... Also you can encapsulate code in comment with appstrophes ` code ` (without the spaces) it will enforce code formatting ... – Spektre Mar 09 '22 at 10:05
Okay done, please let me know if i should change something. – Laurits S Mar 09 '22 at 10:25
@LauritsS Looks OK to me ... – Spektre Mar 09 '22 at 10:49

Calculate 3D arc from three vectors and a given radius

1 Answers1

Linked