/*****************************************************************************/ /* */ /* cgdam.c Cpmputional Giometries for Design and Manufacture */ /* */ /* */ /* Date : April / 29 / 1993 */ /* Copyright (c) 1993 by Kenji Shibata Ver. 1.10a */ /* */ /*****************************************************************************/ /* 【 精度パラメーター "accy" について 】 各々の関数の中で、"double accy = 1.0E-15;" という変数定義があります。 これはオーバーフローを防ぐための概念的な精度パラメータで、除算や sqrt を実行する前などに値を評価するのに使用しています。 例えば、直線の方程式を求める場合、与えられた二点が同一だとエラーにな ります。しかし実際には二点間の距離が非常に小さくてもオーバーフローを起 こす事があります。そこで、引数として与えられる値やシステムの精度によっ て、この精度パラメーターの値を変える必要があります。値は不正な値を検出 でき、かつ有効なデータを損なわないものに設定しなければなりません。その ために事前のテストをお勧めします。目安としては、x86系のシステムでは、 単精度で 1.0E-5 から 7、倍精度で 1.0E-13から15程度で評価してください。 */ #include #include #include /* TO SET UP AN EQUATION OF LINE AND CALCULATE THE COEFFICIENTS ( ax + by + c = 0 ) */ int lnprm( double *pt1, double *pt2, double *a, double *b, double *c ) { double xlk, ylk, rsq, rinv; double accy = 1.0E-15; xlk = pt2[0] - pt1[0]; ylk = pt2[1] - pt1[1]; rsq = pow(xlk, 2.0) + pow(ylk, 2.0); if(rsq < accy){ return (-1); }else{ rinv = 1.0 / sqrt(rsq); *a = -ylk * rinv; *b = xlk * rinv; *c = (pt1[0] * pt2[1] - pt2[0] * pt1[1]) * rinv; } /* end if */ return (0); } /* lnprm */ /* INTERSECTING POINT OF LINES */ int lnln( double (*pxy1)[2], double (*pxy2)[2], double *xy ) { double pt1[2], pt2[2], a1, b1, c1, a2, b2, c2, det, dinv; double accy = 1.0E-15; pt1[0] = pxy1[0][0]; pt1[1] = pxy1[0][1]; pt2[0] = pxy1[1][0]; pt2[1] = pxy1[1][1]; if(lnprm(pt1, pt2, &a1, &b1, &c1)) return (-1); /* 2点は一致する。 */ pt1[0] = pxy2[0][0]; pt1[1] = pxy2[0][1]; pt2[0] = pxy2[1][0]; pt2[1] = pxy2[1][1]; if(lnprm(pt1, pt2, &a2, &b2, &c2)) return (-1); /* 2点は一致する。 */ det = a1*b2 - a2*b1; if(fabs(det) < accy){ return (-1); /* 交点は、存在しない。 */ }else{ dinv = 1.0 / det; xy[0] = ( b1*c2 - b2*c1 ) * dinv; xy[1] = ( a2*c1 - a1*c2) * dinv; } /* end if */ return (0); } /* lnln */ /* INTERSECTING POINTS OF LINE AND CIRCLE */ int lncl( double *pt1, double *pt2, double *xyr, double *pnear, double *xy) { double root, factor, xo, yo, f, g, fsq, gsq, fgsq, xjo, yjo, a, b, c; double fygx, fxgy, t, fginv, t1, t2, x1, y1, x2, y2, sqdst1, sqdst2; double accy = 1.0E-15; if(lnprm(pt1, pt2, &a, &b, &c)) return (-1); root = 1.0 / (a*a + b*b); factor = -c * root; xo = a * factor; yo = b * factor; root = sqrt(root); f = b * root; g = -a * root; fsq = f*f; gsq = g*g; fgsq = fsq+gsq; if(fgsq < accy){ return (3); }else{ xjo = xyr[0] - xo; yjo = xyr[1] - yo; fygx = f*yjo - g*xjo; root = xyr[2]*xyr[2]*fgsq - fygx*fygx; if (root < -accy){ /* 交点なし */ return (-1); }else{ fxgy = f*xjo + g*yjo; if (root < accy){ /* 直線と円は接する。*/ t = fxgy / fgsq; xy[0] = xo + f*t; xy[1] = yo + g*t; return (1); }else{ root = sqrt(root); fginv = 1.0 / fgsq; t1 = (fxgy - root)*fginv; t2 = (fxgy + root)*fginv; x1 = xo + f*t1; y1 = yo + g*t1; x2 = xo + f*t2; y2 = yo + g*t2; } /* end if */ } /* end if */ } /* end if */ sqdst1 = pow((pnear[0] - x1), 2.0) + pow((pnear[1] - y1), 2.0); sqdst2 = pow((pnear[0] - x2), 2.0) + pow((pnear[1] - y2), 2.0); if (sqdst1 < sqdst2){ /* pnearに近い方の交点を返す。*/ xy[0] = x1; xy[1] = y1; }else{ xy[0] = x2; xy[1] = y2; } /* end if */ return (2); } /* lncl */ /* INTERSECTING POINTS OF TWO CIRCLES */ int clcl( double *xyr1, double *xyr2, double *pnear, double *xy) { double rksq, rlsq, xlk, ylk, distsq, delrsq, sumrsq, root, dstinv, scl, xfac, yfac, x, y, x1, y1, x2, y2, sqdst1, sqdst2; double accy = 1.0E-15; rksq = pow(xyr1[2], 2.0); rlsq = pow(xyr2[2], 2.0); xlk = xyr2[0] - xyr1[0]; ylk = xyr2[1] - xyr1[1]; distsq = pow(xlk, 2.0) + pow(ylk, 2.0); if(distsq < accy){ return (-1); }else{ delrsq = rlsq - rksq; sumrsq = rksq + rlsq; root = 2.0*sumrsq*distsq - pow(distsq, 2.0) - pow(delrsq, 2.0); if (root < -accy){ return (-1); }else{ dstinv = 0.5 / distsq; scl = 0.5 - delrsq*dstinv; x = xlk*scl + xyr1[0]; y = ylk*scl + xyr1[1]; if (root < accy){ /* 2つの円は接する。*/ xy[0] = x; xy[1] = y; return (1); }else{ root = dstinv * sqrt(root); xfac = xlk * root; yfac = ylk * root; x1 = x - yfac; y1 = y + xfac; x2 = x + yfac; y2 = y - xfac; } /* end if */ } /* end if */ } /* end if */ sqdst1 = pow((pnear[0]-x1), 2.0) + pow((pnear[1]-y1), 2.0); sqdst2 = pow((pnear[0]-x2), 2.0) + pow((pnear[1]-y2), 2.0); if (sqdst1 < sqdst2){ xy[0] = x1; xy[1] = y1; }else{ xy[0] = x2; xy[1] = y2; } /* end if */ return (2); } /* clcl */ /* CREATING A SEGMENT PARALLE TO A REFERENCE SEGMENT modified May / 25 / 1993 */ int lnoff( double *pt1, double *pt2, double dist, double *xy1, double *xy2 ) { double xlk, ylk, rsq, rinv; double accy = 1.0E-15; xlk = pt2[0] - pt1[0]; ylk = pt2[1] - pt1[1]; rsq = pow(xlk, 2.0) + pow(ylk, 2.0); if(rsq < accy){ return (-1); }else{ rinv = 1.0 / sqrt(rsq); *a = -ylk * rinv * dist; *b = xlk * rinv * dist; *c = (pt1[0] * pt2[1] - pt2[0] * pt1[1]) * rinv; } /* end if */ xy1[0] = pt1[0] + *a; xy1[1] = pt1[1] + *b; xy2[0] = pt2[0] + *a; xy2[1] = pt2[1] + *b; return (0); } /* lnoff */ /* CREATING A CONCENTRIC ARC */ int xarc( double *pt, double *xyr, double *ofxy ) { double xkj, ykj, xjsq, dinv, csca, ssca; double accy = 1.0E-15; xkj = pt[0] - xyr[0]; ykj = pt[1] - xyr[1]; xjsq = pow(xkj, 2.0) + pow(ykj, 2.0); if(xjsq < accy) return (-1); dinv = 1.0 / sqrt(xjsq); csca = xkj * dinv; ssca = ykj * dinv; ofxy[0] = xyr[0] + xyr[2] * csca; ofxy[1] = xyr[1] + xyr[2] * ssca; return (0); } /* xarc */ int arcoff( double *pt1, double *pt2, double *xyr, double off, int dir, double *ofxy1, double *ofxy2 ) { off *= (double)dir; xyr[2] += off; if(xarc( pt1, xyr, ofxy1 )) return (-1); if(xarc( pt2, xyr, ofxy2 )) return (-1); return (0); } /* arcoff */ /* CREATING A CIRCLE PASSING THROUGH THREE POINTS */ int defcir( double (*xy)[2], double *xyr) { double xlk, ylk, xmk, ymk, det, detinv, rlksq, rmksq, xcc, ycc; double accy = 1.0E-15; xlk = xy[1][0] - xy[0][0]; ylk = xy[1][1] - xy[0][1]; xmk = xy[2][0] - xy[0][0]; ymk = xy[2][1] - xy[0][1]; det = xlk * ymk - xmk * ylk; if(fabs(det) < accy){ return (-1); }else{ detinv = 0.5 / det; rlksq = pow(xlk, 2.0) + pow(ylk, 2.0); rmksq = pow(xmk, 2.0) + pow(ymk, 2.0); xcc = detinv * ( rlksq * ymk - rmksq * ylk ); ycc = detinv * ( xlk * rmksq - xmk * rlksq ); xyr[2] = sqrt( pow(xcc, 2.0) + pow(ycc, 2.0) ); xyr[0] = xcc + xy[0][0]; xyr[1] = ycc + xy[0][1]; } /* end if */ return (0); } /* defcir */ int main() { double pxy1[3][2], pxy2[3][2], pt1[2], pt2[2], xy[2], xy1[2], xy2[2], pnear[2],xyr1[3], xyr2[3], a, b, c, dist, off; int dir; printf("\033[2J"); pxy1[0][0] = 49.625; pxy1[0][1] = 57.750; pxy1[1][0] = 77.625; pxy1[1][1] = 82.250; pxy2[0][0] = 65.875; pxy2[0][1] = 81.750; pxy2[1][0] = 110.125; pxy2[1][1] = 61.500; pt1[0] = pxy1[1][0]; pt1[1] = pxy1[1][1]; pt2[0] = pxy1[0][0]; pt2[1] = pxy1[0][1]; if(lnprm(pt1, pt2, &a, &b, &c)){ printf("Error ... cannot define a line\n");/* 2点は一致する。*/ exit(1); } /* end if */ printf("\n 直線の方程式\n"); printf(" a = %10.6f, b = %10.6f, c = %10.6f\n", a, b, c); if(lnln( pxy1, pxy2, xy )){ printf("error .... 交点は、存在しない。\n"); exit(1); } /* end if */ printf("\n 2直線の交点\n"); printf(" x = %10.6f, y = %10.6f\n", xy[0], xy[1]); pt1[0] = 33.125; pt1[1] = -1.25; pt2[0] = 87.375; pt2[1] = 31.75; dist = 10.0; if(lnoff( pt1, pt2, dist, xy1, xy2 )){ printf("Error .... 直線が定義できない。\n"); exit(1); } /* end if */ printf("\n 直線のオフセット\n"); printf(" x1 = %10.6f, y1 = %10.6f\n", xy1[0], xy1[1]); printf(" x2 = %10.6f, y2 = %10.6f\n", xy2[0], xy2[1]); dist = -10.0; if(lnoff( pt1, pt2, dist, xy1, xy2 )){ printf("Error .... 直線が定義できない。\n"); exit(1); } /* end if */ printf(" x3 = %10.6f, y3 = %10.6f\n", xy1[0], xy1[1]); printf(" x4 = %10.6f, y4 = %10.6f\n", xy2[0], xy2[1]); getch(); pt1[0] = 175.5; pt1[1] = 79.0; pt2[0] = 198.75; pt2[1] = 53.75; xyr1[0] = 218.551; xyr1[1] = 51.6982; xyr1[2] = 25.4482; pnear[0] = 190.0; pnear[1] = 58.0; if(lncl( pt1, pt2, xyr1, pnear, xy) < 0){ printf("Error .... 交点は、存在しない。\n"); exit(1); } /* end if */ printf("\n 円と直線の交点\n"); printf(" x = %10.6f, y = %10.6f\n", xy[0], xy[1]); xyr1[0] = 120.75; xyr1[1] = 9.0; xyr1[2] = 10.0; xyr2[0] = 143.25; xyr2[1] = 15.0; xyr2[2] = 20.0; pnear[0] = 120.0; pnear[1] = 18.0; if( clcl( xyr1, xyr2, pnear, xy) < 0){ printf("Error .... 交点は、存在しない。\n"); exit(1); } /* end if */ printf("\n 2つの円の交点\n"); printf(" x1 = %10.6f, y1 = %10.6f\n", xy[0], xy[1]); pnear[0] = 120.0; pnear[1] = 2.0; if( clcl( xyr1, xyr2, pnear, xy) < 0){ printf("Error .... 交点は、存在しない。\n"); exit(1); } /* end if */ printf(" x2 = %10.6f, y2 = %10.6f\n", xy[0], xy[1]); getch(); pxy1[0][0] = 224.25; /* x1 */ pxy1[0][1] = 76.5; /* y1 */ pxy1[1][0] = 194.11; /* x2 */ pxy1[1][1] = 58.7887; /* y2 */ pxy1[2][0] = 194.0; /* x3 */ pxy1[2][1] = 45.0; /* y3 */ if(defcir( pxy1, xyr1)){ printf("Error ... 円は定義できない。\n"); exit(1); } /* end if */ printf("\n 円の３点定義\n"); printf(" x = %10.6f, y = %10.6f, r = %10.6f\n", xyr1[0], xyr1[1], xyr1[2]); pt1[0] = 157.25; pt1[1] = 75.25; pt2[0] = 128.5; pt2[1] = 52.0; xyr1[0] = 146.253; xyr1[1] = 59.4478; xyr1[2] = 14.2521; off = 10; dir = 1; /* 外側にオフセット */ if(arcoff( pt1, pt2, xyr1, off, dir, xy1, xy2 )){ printf("Error ... 円弧は定義できない。\n"); exit(1); } /* end if */ printf("\n 円弧のオフセット\n"); printf(" x1 = %10.6f, y1 = %10.6f\n x2 = %10.6f, y2 = %10.6f\n", xy1[0], xy1[1], xy2[0], xy2[1]); dir = -1; /* 内側にオフセット */ if(arcoff( pt1, pt2, xyr1, off, dir, xy1, xy2 )){ printf("Error ... 円弧は定義できない。\n"); exit(1); } /* end if */ printf(" x1 = %10.6f, y1 = %10.6f\n x2 = %10.6f, y2 = %10.6f\n", xy1[0], xy1[1], xy2[0], xy2[1]); return 0; } /* main */