1  component sphereprobe "Probe a pretend hemisphere";
 2  author "Jeff Epler";
 3  license "GPL";
 4  
 5  pin in signed px;
 6  pin in signed py;
 7  pin in signed pz "\\fBrawcounts\\fR position from software encoder";
 8  
 9  pin in signed cx;
10  pin in signed cy;
11  pin in signed cz "Center of sphere in counts";
12  pin in signed r "Radius of hemisphere in counts";
13  
14  pin out bit probe-out;
15  
16  function _ nofp "update probe-out based on inputs";
17  ;;
18  #undef abs
19  int abs(int x) { if(x < 0) return -x; else return x; }
20  
21  FUNCTION(_) {
22      rtapi_u64 dx = abs(px-cx), dy=abs(py-cy), dz=abs(pz-cz);
23      rtapi_u64 d2 = dx*dx + dy*dy;
24      rtapi_u64 r2 = (rtapi_s64)r*(rtapi_s64)r;
25      if(d2 > r2) {
26          probe_out = pz < cz;
27      } else {
28          d2 += dz*dz;
29          probe_out = d2 <= r2;
30      }
31  }