Finding coordinates x,y,z of circle center point.

RAPID Programming
1

Hello.

Could someone have a code in RAPID for finding coordinates x,y,z of a circle center created from 3 points A,B,C in wobj0.

2

Hi!

If you still have trouble with this you can try the following code. Circle3P function computes circle center, axis and radius for the given three perimeter points p1, p2 and p3. I tested this a bit and it seemed to work fine. Note that this only works on new RobotWare because it uses MatrixSolve function.

BR, Keke

MODULE MainModule
  
  CONST num nEPSILON:=1e-6;
  
  PROC main()
    VAR pos p1;
    VAR pos p2;
    VAR pos p3;
    VAR pos c;
    VAR pos a;
    VAR num r;
    VAR bool b;
    !
    p1:=[200,0,100];
    p2:=[0,0,100];
    p3:=[0,200,100];
    b:=Circle3P(p1,p2,p3,c,a,r);
    !
    Stop;
  ENDPROC
  
  FUNC num Max(num n1, num n2)
    !--------------------------------
    !Return maximum of two numbers
    !--------------------------------
    !
    IF n1 > n2 THEN
      RETURN n1;
    ELSE
      RETURN n2;
    ENDIF
  ENDFUNC
  
  FUNC num PosLength(pos p)
    !--------------------------------
    !Length of a vector (pos)
    !--------------------------------
    !
    VAR num nL;
    nL:=sqrt(Max(0,p.x*p.x+p.y*p.y+p.z*p.z));
    RETURN nL;
  ENDFUNC
  
  FUNC pos CrossProd(pos p1,pos p2)
    !--------------------------------
    !Calculate cross product for vectors p1 and p2.
    !--------------------------------
    !
    VAR pos px;
    px:=[0,0,0];
    px.x:=p1.y*p2.z-p2.y*p1.z;
    px.y:=p2.x*p1.z-p1.x*p2.z;
    px.z:=p1.x*p2.y-p2.x*p1.y;
    RETURN px;
  ENDFUNC
  
  FUNC bool Circle3P(pos p1,pos p2,pos p3,INOUT pos center,INOUT pos axis,INOUT num radius)
    !--------------------------------
    !Calculate center point, axis and radius for a circle formed by 
    !three point p1, p2 and p3. Axis is unit vector.
    !Returns True if found, False if points are on a on a single line (infinite radius).
    !--------------------------------
    !
    VAR pos v12;
    VAR pos v23;
    VAR pos px;
    VAR pos p12mid;
    VAR pos p23mid;
    VAR pos n12;
    VAR pos n23;
    VAR pos diff;
    VAR num l;
    VAR dnum A{3,3};
    VAR dnum b{3};
    VAR dnum x{3};
    !
    center:=[0,0,0];
    axis:=[0,0,0];
    radius:=0;
    !
    v12:=p2-p1;
    v23:=p3-p2;
    px:=CrossProd(v12,v23);
    l:=PosLength(px);
    IF l<nEPSILON THEN
      RETURN FALSE;
    ENDIF
    px:=(1.0/l)*px;
    axis:=px;
    !
    p12mid:=p1+0.5*v12;
    p23mid:=p2+0.5*v23;
    n12:=CrossProd(v12,axis);
    n23:=CrossProd(v23,axis);
    !
    A:=[[NumToDnum(v12.x),NumToDnum(v12.y),NumToDnum(v12.z)],
       [NumToDnum(v23.x),NumToDnum(v23.y),NumToDnum(v23.z)],
       [NumToDnum(axis.x),NumToDnum(axis.y),NumToDnum(axis.z)]];
    b:=[NumToDnum(DotProd(v12,p12mid)),NumToDnum(DotProd(v23,p23mid)),NumToDnum(DotProd(axis,p2))];
    MatrixSolve A,b,x;
    !
    center:=[DnumToNum(x{1}),DnumToNum(x{2}),DnumToNum(x{3})];
    diff:=center-p1;
    radius:=PosLength(diff);
    !
    RETURN TRUE;
  ENDFUNC
ENDMODULE