home *** CD-ROM | disk | FTP | other *** search
/ Photo CD Demo 1 / Demo.bin / gems / gemsiii / fillet.c < prev    next >
C/C++ Source or Header  |  1992-04-02  |  5KB  |  208 lines
  1. /*
  2. Joining Two Lines with a Circular Arc Fillet
  3. Robert D. Miller
  4. */
  5.  
  6.  
  7.  
  8.  
  9.  
  10.  
  11.  
  12.  
  13.  
  14.  
  15.  
  16.  
  17.  
  18.  
  19. #include "math.h"
  20. #include "stdio.h"
  21. #include "stdlib.h"
  22.  
  23. #define pi        3.14159265358974
  24. #define halfpi    1.5707963267949
  25. #define threepi2  4.71238898038469
  26. #define twopi     6.28318530717959
  27. #define degree    57.29577951308232088
  28. #define radian    0.01745329251994329577
  29.  
  30. typedef struct point { float x; float y;} point;
  31.  
  32. point   p1, p2, p3, p4, v1, v2;
  33. float   r, xx, yy, sa, sb,
  34.         qx, qy, rx, ry;
  35. int     x1, yy1, x2, y2;
  36.  
  37. float   arccos();
  38. float   arcsin();
  39. float   cross2();
  40. float   dot2();
  41.  
  42. void    moveto();   /* External draw routine */
  43. void    lineto();   /* External draw routine */
  44. void    drawarc();
  45. float   linetopoint();
  46. void    pointperp();
  47. void    fillet();
  48.  
  49. float cross2(v1,v2)
  50.       point v1, v2;
  51. { return (v1.x*v2.y - v2.x*v1.y); }
  52.  
  53. float dot2(v1,v2)   /* Return angle subtended by two vectors.  */
  54.       point v1, v2;
  55. { float   d, t;
  56.  
  57.       d= (float) sqrt(((v1.x*v1.x)+(v1.y*v1.y)) * ((v2.x*v2.x)+(v2.y*v2.y)));
  58.       if (d != (float) 0.0)
  59.       {
  60.          t= (v1.x*v2.x+v1.y*v2.y)/d;
  61.          return (arccos(t));
  62.       }
  63.       else
  64.       return ((float) 0.0);
  65. }
  66.  
  67.  
  68. /* Draw circular arc in one degree increments. Center is (xc,yc)
  69.         with radius r, beginning at starting angle, startang
  70.         through angle ang. If ang < 0 arc is draw clockwise. */
  71.  
  72. void drawarc(xc, yc, r, startang, ang)
  73. float xc, yc, r, startang, ang;
  74.  
  75. {
  76. #define   sindt 0.017452406
  77. #define   cosdt 0.999847695
  78.  
  79. float   a, x, y, xt, yt, sr;
  80. int     k;
  81.  
  82.         a= (float) startang*radian;
  83.         x= (float) r*cos(a);
  84.         y= (float) r*sin(a);
  85.         moveto(xc+x, yc+y, 3);
  86.         if (ang >= (float) 0.0)
  87.            sr= (float) sindt;
  88.         else
  89.            sr= (float) -sindt;
  90.         for (k= 1; k <= (int) floor(fabs(ang)); k++)
  91.           {  x= x*cosdt-y*sr;
  92.              y= x*sr+y*cosdt;
  93.              lineto(xc+x,yc+y);
  94.           }
  95. return;
  96. }
  97.  
  98. /*        Find a,b,c in Ax + By + C = 0  for line p1,p2.         */
  99.  
  100. void linecoef(a,b,c,p1,p2)
  101. float   *a, *b, *c;
  102. point   p1, p2;
  103.  
  104. {
  105.         *c=  (p2.x*p1.y)-(p1.x*p2.y);
  106.         *a= p2.y-p1.y;
  107.         *b= p1.x-p2.x;
  108. return;
  109. }
  110.  
  111. /*       Return signed distance from line Ax + By + C = 0 to point P. */
  112.  
  113. float linetopoint(a,b,c,p)
  114. float   a, b, c;
  115. point   p;
  116.  
  117. {
  118. float   d, lp;
  119.  
  120.         d= sqrt((a*a)+(b*b));
  121.         if (d == 0.0)
  122.            lp =  0.0;
  123.         else
  124.            lp= (a*p.x+b*p.y+c)/d;
  125. return ((float) lp);
  126. }
  127.  
  128. /*   Given line l = ax + by + c = 0 and point p,
  129.      compute x,y so p(x,y) is perpendicular to l. */
  130.  
  131. void pointperp(x, y, a, b, c, p)
  132. float     *x, *y, a, b, c;
  133. point     p;
  134.  
  135. {
  136. float    d, cp, t, u;
  137.  
  138.         *x=  0.0;
  139.         *y=  0.0;
  140.         d=  a*a +b*b;
  141.         cp= a*p.y-b*p.x;
  142.         if (d != 0.0)
  143.           {
  144.              *x= (-a*c-b*cp)/d;
  145.              *y= (a*cp-b*c)/d;
  146.           }
  147. return;
  148. }
  149.  
  150. /*      Compute a circular arc fillet between lines L1 (p1 to p2) and
  151.     L2 (p3 to p4) with radius R.  The circle center is xc,yc.      */
  152.  
  153. void fillet(p1, p2, p3, p4, r, xc, yc, pa, aa)
  154. point   *p1, *p2, *p3, *p4;
  155. float   r, *xc, *yc, *pa, *aa;
  156.  
  157. {
  158. float a1, b1, c1, a2, b2, c2, c1p,
  159.       c2p, d1, d2, xa, xb, ya, yb, d, rr;
  160. point mp, pc;
  161.  
  162.         linecoef(a1,b1,c1,*p1,*p2);
  163.         linecoef(a2,b2,c2,*p3,*p4);
  164.  
  165.         if ((a1*b2) == (a2*b1))  /* Parallel or coincident lines /*
  166.             goto xit;
  167.  
  168.         mp.x= ((*p3).x + (*p4).x)/2.0;
  169.         mp.y= ((*p3).y + (*p4).y)/2.0;
  170.         d1= linetopoint(a1,b1,c1,mp);  /* Find distance p1p2 to p3 */
  171.         if (d1 == 0.0) goto xit;
  172.  
  173.         mp.x= ((*p1).x + (*p2).x)/2.0;
  174.         mp.y= ((*p1).y + (*p2).y)/2.0;
  175.         d2= linetopoint(a2,b2,c2,mp);  /* Find distance p3p4 to p2 */
  176.         if (d2 == 0.0) goto xit;
  177.  
  178.         rr= r;
  179.         if (d1 <= 0.0) rr= -rr;
  180.  
  181.         c1p= c1-rr*sqrt((a1*a1)+(b1*b1));  /* Line parallel l1 at d */
  182.         rr= r;
  183.  
  184.         if (d2 <= 0.0) rr= -rr;
  185.  
  186.         c2p= c2-rr*sqrt((a2*a2)+(b2*b2));  /* Line parallel l2 at d */
  187.         d= a1*b2-a2*b1;
  188.  
  189.         *xc= (c2p*b1-c1p*b2)/d;            /* Intersect constructed lines */
  190.         *yc= (c1p*a2-c2p*a1)/d;            /* to find center of arc */
  191.         pc.x=  *xc;
  192.         pc.y=  *yc;
  193.  
  194.         pointperp(xa,ya,a1,b1,c1,pc);      /* Clip or extend lines as required */
  195.         pointperp(xb,yb,a2,b2,c2,pc);
  196.         (*p2).x= xa; (*p2).y= ya;
  197.         (*p3).x= xb; (*p3).y= yb;
  198.         v1.x= xa-*xc; v1.y= ya-*yc;
  199.         v2.x= xb-*xc; v2.y= yb-*yc;
  200.  
  201.         *pa= (float) atan2(v1.y,v1.x);     /* Beginning angle for arc */
  202.         *aa= dot2(v1,v2);
  203.         if (cross2(v1,v2) < 0.0) *aa= -*aa; /* Angle subtended by arc */
  204.  
  205. xit:
  206. return;
  207. }
  208.