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

kocykowaty

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.

Keke

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