fillet.c

[Game.Programming].Academic - Graphics Gems (6 books source code)

/*
Joining Two Lines with a Circular Arc Fillet
Robert D. Miller
*/














#include "math.h"
#include "stdio.h"
#include "stdlib.h"

#define pi        3.14159265358974
#define halfpi    1.5707963267949
#define threepi2  4.71238898038469
#define twopi     6.28318530717959
#define degree    57.29577951308232088
#define radian    0.01745329251994329577

typedef struct point { float x; float y;} point;

point   gv1, gv2;
float   xx, yy, sa, sb,
        qx, qy, rx, ry;
int     x1, yy1, x2, y2;

float   arccos();
float   arcsin();
float   cross2();
float   dot2();

void    moveto();   /* External draw routine */
void    lineto();   /* External draw routine */
void    drawarc();
float   linetopoint();
void    pointperp();
void    fillet();

float cross2(v1,v2)
      point v1, v2;
{ return (v1.x*v2.y - v2.x*v1.y); }

float dot2(v1,v2)   /* Return angle subtended by two vectors.  */
      point v1, v2;
{ float   d, t;

      d= (float) sqrt(((v1.x*v1.x)+(v1.y*v1.y)) * ((v2.x*v2.x)+(v2.y*v2.y)));
      if (d != (float) 0.0)
      {
         t= (v1.x*v2.x+v1.y*v2.y)/d;
         return (arccos(t));
      }
      else
      return ((float) 0.0);
}


/* Draw circular arc in one degree increments. Center is (xc,yc)
        with radius r, beginning at starting angle, startang
        through angle ang. If ang < 0 arc is draw clockwise. */

void drawarc(xc, yc, r, startang, ang)
float xc, yc, r, startang, ang;

{
#define   sindt 0.017452406
#define   cosdt 0.999847695

float   a, x, y, sr;
int     k;

        a= (float) startang*radian;
        x= (float) r*cos(a);
        y= (float) r*sin(a);
        moveto(xc+x, yc+y, 3);
        if (ang >= (float) 0.0)
           sr= (float) sindt;
        else
           sr= (float) -sindt;
        for (k= 1; k <= (int) floor(fabs(ang)); k++)
          {  x= x*cosdt-y*sr;
             y= x*sr+y*cosdt;
             lineto(xc+x,yc+y);
          }
return;
}

/*        Find a,b,c in Ax + By + C = 0  for line p1,p2.         */

void linecoef(a,b,c,p1,p2)
float   *a, *b, *c;
point   p1, p2;

{
        *c=  (p2.x*p1.y)-(p1.x*p2.y);
        *a= p2.y-p1.y;
        *b= p1.x-p2.x;
return;
}

/*       Return signed distance from line Ax + By + C = 0 to point P. */

float linetopoint(a,b,c,p)
float   a, b, c;
point   p;

{
float   d, lp;

        d= sqrt((a*a)+(b*b));
        if (d == 0.0)
           lp =  0.0;
        else
           lp= (a*p.x+b*p.y+c)/d;
return ((float) lp);
}

/*   Given line l = ax + by + c = 0 and point p,
     compute x,y so p(x,y) is perpendicular to l. */

void pointperp(x, y, a, b, c, p)
float     *x, *y, a, b, c;
point     p;

{
float    d, cp;

        *x=  0.0;
        *y=  0.0;
        d=  a*a +b*b;
        cp= a*p.y-b*p.x;
        if (d != 0.0)
          {
             *x= (-a*c-b*cp)/d;
             *y= (a*cp-b*c)/d;
          }
return;
}

/*      Compute a circular arc fillet between lines L1 (p1 to p2) and
	L2 (p3 to p4) with radius R.  The circle center is xc,yc.      */

void fillet(p1, p2, p3, p4, r, xc, yc, pa, aa)
point   *p1, *p2, *p3, *p4;
float   r, *xc, *yc, *pa, *aa;

{
float a1, b1, c1, a2, b2, c2, c1p,
      c2p, d1, d2, xa, xb, ya, yb, d, rr;
point mp, pc;

        linecoef(&a1,&b1,&c1,*p1,*p2);
        linecoef(&a2,&b2,&c2,*p3,*p4);

        if ((a1*b2) == (a2*b1))  /* Parallel or coincident lines /*
            goto xit;

        mp.x= ((*p3).x + (*p4).x)/2.0;
        mp.y= ((*p3).y + (*p4).y)/2.0;
        d1= linetopoint(a1,b1,c1,mp);  /* Find distance p1p2 to p3 */
        if (d1 == 0.0) goto xit;

        mp.x= ((*p1).x + (*p2).x)/2.0;
        mp.y= ((*p1).y + (*p2).y)/2.0;
        d2= linetopoint(a2,b2,c2,mp);  /* Find distance p3p4 to p2 */
        if (d2 == 0.0) goto xit;

        rr= r;
        if (d1 <= 0.0) rr= -rr;

        c1p= c1-rr*sqrt((a1*a1)+(b1*b1));  /* Line parallel l1 at d */
        rr= r;

        if (d2 <= 0.0) rr= -rr;

        c2p= c2-rr*sqrt((a2*a2)+(b2*b2));  /* Line parallel l2 at d */
        d= a1*b2-a2*b1;

        *xc= (c2p*b1-c1p*b2)/d;            /* Intersect constructed lines */
        *yc= (c1p*a2-c2p*a1)/d;            /* to find center of arc */
        pc.x=  *xc;
        pc.y=  *yc;

        pointperp(&xa,&ya,a1,b1,c1,pc);      /* Clip or extend lines as required */
        pointperp(&xb,&yb,a2,b2,c2,pc);
        (*p2).x= xa; (*p2).y= ya;
        (*p3).x= xb; (*p3).y= yb;
        gv1.x= xa-*xc; gv1.y= ya-*yc;
        gv2.x= xb-*xc; gv2.y= yb-*yc;

        *pa= (float) atan2(gv1.y,gv1.x);     /* Beginning angle for arc */
        *aa= dot2(gv1,gv2);
        if (cross2(gv1,gv2) < 0.0) *aa= -*aa; /* Angle subtended by arc */

xit:
return;
}