通过三点求圆心程序(二维和三维两种方式),代码为ABB机器人程序,其他语言也适用

三维空间程序


 RECORD CircleData
    pos PD;
    num radius;
 ENDRECORD

FUNC CircleData SearchCircle(pos P1,pos P2,pos P3)
        VAR num a1;
        VAR num b1;
        VAR num c1;
        VAR num d1;
        VAR num a2;
        VAR num b2;
        VAR num c2;
        VAR num d2;
        VAR num a3;
        VAR num b3;
        VAR num c3;
        VAR num d3;

        VAR num x1;
        VAR num y1;
        VAR num z1;
        VAR num x2;
        VAR num y2;
        VAR num z2;
        VAR num x3;
        VAR num y3;
        VAR num z3;
        VAR CircleData CD;

        x1:=p1.x;
        y1:=p1.y;
        z1:=p1.z;
        x2:=p2.x;
        y2:=p2.y;
        z2:=p2.z;
        x3:=p3.x;
        y3:=p3.y;
        z3:=p3.z;
        a1:=(y1*z2-y2*z1-y1*z3+y3*z1+y2*z3-y3*z2);
        b1:=-(x1*z2-x2*z1-x1*z3+x3*z1+x2*z3-x3*z2);
        c1:=(x1*y2-x2*y1-x1*y3+x3*y1+x2*y3-x3*y2);
        d1:=-(x1*y2*z3-x1*y3*z2-x2*y1*z3+x2*y3*z1+x3*y1*z2-x3*y2*z1);

        a2:=2*(x2-x1);
        b2:=2*(y2-y1);
        c2:=2*(z2-z1);
        d2:=x1*x1+y1*y1+z1*z1-x2*x2-y2*y2-z2*z2;

        a3:=2*(x3-x1);
        b3:=2*(y3-y1);
        c3:=2*(z3-z1);
        d3:=x1*x1+y1*y1+z1*z1-x3*x3-y3*y3-z3*z3;

        cd.PD.x:=-(b1*c2*d3-b1*c3*d2-b2*c1*d3+b2*c3*d1+b3*c1*d2-b3*c2*d1)
        /(a1*b2*c3-a1*b3*c2-a2*b1*c3+a2*b3*c1+a3*b1*c2-a3*b2*c1);
        cd.PD.y:=(a1*c2*d3-a1*c3*d2-a2*c1*d3+a2*c3*d1+a3*c1*d2-a3*c2*d1)
        /(a1*b2*c3-a1*b3*c2-a2*b1*c3+a2*b3*c1+a3*b1*c2-a3*b2*c1);
        cd.PD.z:=-(a1*b2*d3-a1*b3*d2-a2*b1*d3+a2*b3*d1+a3*b1*d2-a3*b2*d1)
        /(a1*b2*c3-a1*b3*c2-a2*b1*c3+a2*b3*c1+a3*b1*c2-a3*b2*c1);

        cd.radius:=sqrt((x1-cd.PD.x)*(x1-cd.PD.x)+(y1-cd.PD.y)*(y1-cd.PD.y)+(z1-cd.PD.z)*(z1-cd.PD.z));
        cd.radius:=sqrt((x2-cd.PD.x)*(x2-cd.PD.x)+(y2-cd.PD.y)*(y2-cd.PD.y)+(z2-cd.PD.z)*(z2-cd.PD.z));
        cd.radius:=sqrt((x3-cd.PD.x)*(x3-cd.PD.x)+(y3-cd.PD.y)*(y3-cd.PD.y)+(z3-cd.PD.z)*(z3-cd.PD.z));

        RETURN CD;
    ENDFUNC

二维空间程序

RECORD CircleData
    pos PD;
    num radius;
ENDRECORD

FUNC CircleData SearchCircle(pos P1,pos P2,pos P3)
        VAR pos MidP1;
        VAR pos MidP2;
        VAR num K1:=0;
        VAR num K2:=0;
        VAR num MinZ;
        VAR num MaxZ;
        VAR CircleData CD;
        k1 := -(P2.x - P1.x) / (P2.y - P1.y);
        k2 := -(P3.x - P1.x) / (P3.y - P1.y);
        CD.PD.x := (MidP2.y - MidP1.y - k2* MidP2.x + k1*MidP1.x) / (k1 - k2);
        CD.PD.y := MidP1.y + k1*(MidP2.y - MidP1.y - k2*MidP2.x + k2*MidP1.x) / (k1 - k2);
        CD.radius := sqrt((CD.PD.x - P1.x)*(CD.PD.x - P1.x) + (CD.PD.y - P1.y)*(CD.PD.y - P1.y));
        RETURN CD;
    ENDFUNC
心之所向 素履以往
原文地址:https://www.cnblogs.com/RobotCode/p/14653109.html