C# : Find all points on an arc formed by 3 points (3D)

Question

I want to find the coordonnees of all the point situated on a arc formed by 3 points.

First of all, I don't speak very wel english, so sorry for the language's mistaken.

Point 1 : (toolPosition.x, toolPosition.y, toolPosition.z) Point 2 : (TMid.transform.position.x, TMid.transform.position.y, TMid.transform.position.z) Point 3 : (TEnd.transform.position.x, TEnd.transform.position.y, TEnd.transform.position.z)

I'm in a (x,y,z) coord, where is the difficulty.

I started by calculate the coord of the center of the sphere (xc, yc, zc), that point works.

After, I calculated the length and the angle of the arc (Point 1 to Point 3, go through the Point 2)

//calcul des angles des points donnés sur la sphère
double tetaA = Math.Acos((toolPosition.z - zc) / rayon);
double tetaB = Math.Acos((TMid.transform.position.z - zc) / rayon);
double tetaC = Math.Acos((TEnd.transform.position.z - zc) / rayon);

double phiA = Math.Atan2((toolPosition.y - yc), (toolPosition.x - xc));
double phiB = Math.Atan2((TMid.transform.position.y - yc), (TMid.transform.position.x - xc));
double phiC = Math.Atan2((TEnd.transform.position.y - yc), (TEnd.transform.position.x - xc));
//longueur des arcs
double longStartEnd = rayon * Math.Acos(Math.Cos(tetaA) * Math.Cos(tetaC) + Math.Sin(tetaA) * Math.Sin(tetaC) * Math.Cos(phiC - phiA));
double longMidEnd = rayon * Math.Acos(Math.Cos(tetaB) * Math.Cos(tetaC) + Math.Sin(tetaB) * Math.Sin(tetaC) * Math.Cos(phiC - phiB));
double longStartMid = rayon * Math.Acos(Math.Cos(tetaA) * Math.Cos(tetaB) + Math.Sin(tetaA) * Math.Sin(tetaB) * Math.Cos(phiB - phiA));
//si le trajet dépasse les 180°
if (longStartMid > longStartEnd || ((float)(longStartEnd * 2) != (float)(2 * Math.PI * rayon) && (float)(longStartMid + longStartEnd + longMidEnd) == (float)(2 * Math.PI * rayon)))
    longStartEnd = 2 * Math.PI * rayon - longStartEnd;
//calcul du nombre de découpe à réaliser et de l'angle formé par chacune
int nbrDecoupe = (int)(longStartEnd * 100);
double angle = longStartEnd / rayon;
double angleBeta = angle / nbrDecoupe;

Now come my real problem, find all the points on the arc.

I've tried to calculated them by a rotation matrix, which need a normal vector.

Here is how I calculated it :

//recherche du vecteur normal du plan formé par les trois points
double xsm = TMid.transform.position.x - toolPosition.x, ysm = TMid.transform.position.y - toolPosition.y, zsm = TMid.transform.position.z - toolPosition.z; // vecteur entre le startPoint et le midPoint
double xse = TEnd.transform.position.x - toolPosition.x, yse = TEnd.transform.position.y - toolPosition.y, zse = TEnd.transform.position.z - toolPosition.z; // vecteur entre le startPoint et le endPoint
double z1 = zse / zsm;
if (Math.Sign(zsm) == Math.Sign(zse))
    z1 *= -1;
double y1 = yse / ysm;
if (Math.Sign(ysm) == Math.Sign(yse))
    y1 *= -1;
//vecteur normal
double aNormal = 1;
double bNormal = (z1 * xsm + xse) * -1 / (z1 * ysm + yse);
double cNormal = (y1 * xsm + xse) * -1 / (y1 * zsm + zse);
//vecteur normal unitaire
double ux, uy , uz; // ux² + uy² + uz² = 1
ux = 1 / Math.Sqrt(Math.Pow(aNormal, 2) + Math.Pow(bNormal, 2) + Math.Pow(cNormal, 2)) * aNormal * sens;
uy = 1 / Math.Sqrt(Math.Pow(aNormal, 2) + Math.Pow(bNormal, 2) + Math.Pow(cNormal, 2)) * bNormal * sens;
uz = 1 / Math.Sqrt(Math.Pow(aNormal, 2) + Math.Pow(bNormal, 2) + Math.Pow(cNormal, 2)) * cNormal * sens;

The first point is that, sometimes (when the arc is on an axe) bNormal or cNormal are divided by 0.

The second point is that I don't now exactly how to determinate the sens of the vector (the arc must go through the Point 2).

I've done this to find the sens but I know this is not a good solution

int sens;
if ((TEnd.transform.position.y < toolPosition.y || TEnd.transform.position.z < toolPosition.z) && Math.Abs(TEnd.transform.position.y - toolPosition.y) > 0.00001)
    sens = -1;
else
    sens = 1;
if (Math.Abs(toolPosition.y - TEnd.transform.position.y) < Math.Abs(toolPosition.y - TMid.transform.position.y))
    sens *= -1;

With all that data, I calculated all the point :

 double xL = toolPosition.x - xc, yL = toolPosition.y - yc, zL = toolPosition.z - zc;
 for (int i = 0; i < nbrDecoupe; i++)
 {
     //Coordonnées du nouveau point
     double tempX = xL, tempY = yL;
     // xL * (cos() + ux²*(1 - cos()))      + yL * (ux*uy*(1 - cos()) - uz*sin()) + zL * (ux*uz*(1 - cos()) + uy*sin())
     // xL * (uy*ux*(1 - cos()) + uz*sin()) + yL * (cos() + uy²*(1 - cos()))      + zL * (uy*uz*(1 - cos()) - ux*sin())
     // xL * (uz*ux*(1 - cos()) - uy*sin()) + yL * (uz*uy*(1 - cos()) + ux*sin()) + zL * (cos() + uz²*(1 - cos()))
     xL = tempX * (Math.Cos(angleBeta) + Math.Pow(ux, 2) * (1 - Math.Cos(angleBeta))) + tempY * (ux * uy * (1 - Math.Cos(angleBeta)) - uz * Math.Sin(angleBeta)) + zL * (ux * uz * (1 - Math.Cos(angleBeta)) + uy * Math.Sin(angleBeta));   
     yL = tempX * (uy * ux * (1 - Math.Cos(angleBeta)) + uz * Math.Sin(angleBeta)) + tempY * (Math.Cos(angleBeta) + Math.Pow(uy, 2) * (1 - Math.Cos(angleBeta))) + zL * (uy * uz * (1 - Math.Cos(angleBeta)) - ux * Math.Sin(angleBeta));   
     zL = tempX * (uz * ux * (1 - Math.Cos(angleBeta)) - uy * Math.Sin(angleBeta)) + tempY * (uz * uy * (1 - Math.Cos(angleBeta)) + ux * Math.Sin(angleBeta)) + zL * (Math.Cos(angleBeta) + Math.Pow(uz, 2) * (1 - Math.Cos(angleBeta)));
 }

I think the problem come from the normal vector which is not good sometimes.

So, the two points on which I need help is how to determinate the sens and the calcul of the vector (which don't work in all the situation actually)

If someone can help me, it will be very thankfully.

Answer 1

You can find circle center and radius of arc as described here (another implementation). This is C++ code, but I think that translation should be obvious (dot is dot product and .norm() is normalization of vector)

void estimate3DCircle(Vector3D p1, Vector3D p2, Vector3D p3, Vector3D &c, double &radius)
{
    Vector3D v1 = p2-p1;
    Vector3D v2 = p3-p1;
    double v1v1, v2v2, v1v2;
    v1v1 = v1.dot(v1);
    v2v2 = v2.dot(v2);
    v1v2 = v1.dot(v2);

    float base = 0.5/(v1v1*v2v2-v1v2*v1v2);
    float k1 = base*v2v2*(v1v1-v1v2);
    float k2 = base*v1v1*(v2v2-v1v2);
    c = p1 + v1*k1 + v2*k2; // center

    radius = (c-p1).norm();
}

When you have center, it is not hard to generate points at the arc with needed step (note that you cannot make "all points" - there is infinite number).

Simple approach to generate points with angle/distance step is using SLERP. At first get vectors

p0 = Start - Center
 and 
p1 = End - Center 

Then calculate Omega (full arc angle) using acos and dot product

Omega = acos(p0.dot.p1 / R^2)

then apply interpolation by t parameter in range 0..1

slerp(t) = p0 * sin((1-t)*Omega) / sin(Omega) + p1 * sin(t*Omega) / sin(Omega)