Zirkulares Conveyor Trecking

!*********************************************************
  ! Function:  UTL_cirCntr
  ! Description:
  !   The centre of a circle  is calculated from three positions on 
  !   the circumference.
  !
  !*********************************************************
  FUNC robtarget UTL_cirCntr(
    robtarget p1,
    robtarget p2,
    robtarget p3)


    VAR pos p12;
    VAR pos p13;
    VAR pos p23;
    VAR pos k2;
    VAR pos pn12;
    VAR pos pn13;
    VAR num a;
    VAR num b;
    VAR num ai;
    VAR num bi;
    VAR num c;
    VAR num d;
    VAR num s;
    VAR num r;
    VAR num kl;
    VAR robtarget p0;


    p12:=p2.trans-p1.trans;
    p13:=p3.trans-p1.trans;
    p23:=p3.trans-p2.trans;
    a:=Sqrt(p12.x*p12.x+p12.y*p12.y+p12.z*p12.z);
    b:=Sqrt(p13.x*p13.x+p13.y*p13.y+p13.z*p13.z);
    c:=Sqrt(p23.x*p23.x+p23.y*p23.y+p23.z*p23.z);
    s:=(a+b+c)/2;
    ai:=1/a;
    bi:=1/b;
    r:=0.25*a*b*c/Sqrt(s*(s-a)*(s-b)*(s-c));
    IF a>b THEN
      p23:=p13;
      p13:=p12;
      p12:=p23;
      a:=b;
    ENDIF
    pn12:=p12*ai;
    pn13:=p13*bi;
    k2:=pn12*pn13*pn12;
    kl:=1/Sqrt(k2.x*k2.x+k2.y*k2.y+k2.z*k2.z);
    k2:=kl*k2;
    d:=Sqrt(r*r-a*a/4);
    p0:=p1;
    p0.trans:=p1.trans+0.5*p12+d*k2;
    RETURN p0;
  ENDFUNC