intersection.c
Go to the documentation of this file.
00001 /* intersection.c
00002    finds an intersection point of two lines
00003    (c)1987, MATSUI T., ETL
00004    
00005    1998--
00006    Since this file is compiled and linked in the libeusgeo.so library,
00007    this must be compiled with the -fpic (position independent) option.
00008 
00009 */
00010 
00011 #include "../c/eus.h"
00012 #pragma init (init_object_module)
00013 extern pointer intersection();
00014 
00015 static init_object_module()
00016   { add_module_initializer("intersection", intersection);}
00017 
00018 pointer LINEINTERSECTION(ctx,n,argv)
00019 register context *ctx;
00020 int n;
00021 register pointer argv[];
00022 {
00023   eusfloat_t cz,u,v;
00024   eusfloat_t a1x,a1y,b1x,b1y;
00025   register eusfloat_t ax,ay,bx,by,abx,aby;
00026   int range_check;
00027   pointer up,vp;  
00028   numunion nu;
00029 
00030   ckarg2(4,5);
00031   if (!isfltvector(argv[0])) error(E_FLOATVECTOR,NULL);
00032   if (!isfltvector(argv[1])) error(E_FLOATVECTOR,NULL);
00033   if (!isfltvector(argv[2])) error(E_FLOATVECTOR,NULL);
00034   if (!isfltvector(argv[3])) error(E_FLOATVECTOR,NULL);
00035   if (n>4 && (argv[4]!=NIL)) range_check=1; else range_check=0;
00036   a1x=argv[0]->c.fvec.fv[0]; a1y=argv[0]->c.fvec.fv[1];
00037   b1x=argv[2]->c.fvec.fv[0]; b1y=argv[2]->c.fvec.fv[1];
00038   ax=argv[1]->c.fvec.fv[0]-a1x; ay=argv[1]->c.fvec.fv[1]-a1y;
00039   bx=argv[3]->c.fvec.fv[0]-b1x; by=argv[3]->c.fvec.fv[1]-b1y;
00040   abx=b1x-a1x; aby=b1y-a1y;
00041   cz=ax*by-ay*bx;
00042   if (cz==0) return(NIL);       /* parallel  Nov.1991*/
00043   u=(abx*by-aby*bx)/cz;
00044   v=(abx*ay-aby*ax)/cz;
00045   if (range_check && 
00046         (u<0.0 || 1.0<u || v<0.0 || 1.0<v)) return(NIL);
00047   else {
00048     up=makeflt(u); vp=makeflt(v);
00049     /* printf("%f %f\n",u,v); */
00050     return(cons(ctx,up,cons(ctx,vp,NIL))); }  }
00051 
00052 /* (defun det3 (a b c)
00053 /*  %(a[0] * b[1] * c[2] - a[2] * b[1] * c[0] +
00054 /*    a[1] * b[2] * c[0] - a[1] * b[0] * c[2] +
00055 /*    a[2] * b[0] * c[1] - a[0] * b[2] * c[1]))
00056 /*
00057 /* (defun line-intersection3 (a1 a2 b1 b2 &optional tol)
00058 /*   (let* ((p1 a1) (v1 (normalize-vector (v- a2 a1)))
00059 /*        (p2 b1) (v2 (normalize-vector (v- b2 b1))) 
00060 /*        (p2-p1 (v- p2 p1))
00061 /*        (cross (v* v1 v2))
00062 /*        (cross2 (v. cross cross))
00063 /*        t1 t2)
00064 /*      (cond ((< cross2 0.001)  nil)
00065 /*          (t (list (/ (det3 p2-p1 v2 cross) cross2)
00066 /*                   (/ (det3 p2-p1 v1 cross) cross2))))) )
00067 */
00068 
00069 
00070 
00071 static eusfloat_t determinant3(a,b,c)
00072 eusfloat_t *a, *b, *c;
00073 { return(a[0] * b[1] * c[2] - a[2] * b[1] * c[0] +
00074          a[1] * b[2] * c[0] - a[1] * b[0] * c[2] +
00075          a[2] * b[0] * c[1] - a[0] * b[2] * c[1]);}
00076 
00077 static eusfloat_t *crossproduct(a,b,r)
00078 register eusfloat_t *a, *b, *r;
00079 { r[0]=a[1] * b[2] - a[2] * b[1];
00080   r[1]=a[2] * b[0] - a[0] * b[2];
00081   r[2]=a[0] * b[1] - a[1] * b[0];
00082   return(r);}
00083 
00084 
00085 pointer LINEINTERSECTION3(ctx,n,argv)
00086 register context *ctx;
00087 int n;
00088 register pointer argv[];
00089 { register eusfloat_t *fv;
00090   eusfloat_t tolerance;
00091   eusfloat_t cz,u,v;
00092   eusfloat_t *p1, v1[3], *p2, v2[3], p2p1[3];
00093   eusfloat_t cross[3], cross2;
00094   numunion nu;
00095 
00096   ckarg2(4,5);
00097   if (!isfltvector(argv[0])) error(E_FLOATVECTOR,NULL);
00098   if (!isfltvector(argv[1])) error(E_FLOATVECTOR,NULL);
00099   if (!isfltvector(argv[2])) error(E_FLOATVECTOR,NULL);
00100   if (!isfltvector(argv[3])) error(E_FLOATVECTOR,NULL);
00101   if (n==5) tolerance=ckfltval(argv[4]); else tolerance=0.0;
00102   
00103   p1=argv[0]->c.fvec.fv;
00104   fv=argv[1]->c.fvec.fv;
00105         v1[0]=fv[0]-p1[0]; v1[1]=fv[1]-p1[1]; v1[2]=fv[2]-p1[2];
00106 
00107   p2=argv[2]->c.fvec.fv;
00108   fv=argv[3]->c.fvec.fv;
00109         v2[0]=fv[0]-p2[0]; v2[1]=fv[1]-p2[1]; v2[2]=fv[2]-p2[2];
00110 
00111   p2p1[0]=p2[0]-p1[0];  p2p1[1]=p2[1]-p1[1];  p2p1[2]=p2[2]-p1[2];
00112   crossproduct(v1,v2,cross);
00113   cross2=cross[0]*cross[0] + cross[1]*cross[1] + cross[2]*cross[2];
00114   if (cross2<tolerance) return(NIL);
00115 
00116   u=determinant3(p2p1,v2,cross)/cross2;
00117   v=determinant3(p2p1,v1,cross)/cross2;
00118   return(cons(ctx,makeflt(u),cons(ctx,makeflt(v),NIL))); }
00119 
00120 
00121 /*
00122 /*      viewport clipping functions
00123 /*      VPCLIP is for normal coordinate system
00124 /*      HOMO_VPCLIP is for homogeneous coordinate system
00125 /*
00126 /*      rewritten in 1987-Jun
00127 /*      Copyright(1986) Toshihiro MATSUI
00128 */
00129 
00130 extern pointer makefvector();
00131 
00132 #define clipcode(x,y,z)  (((x)<-(z))?1:(((x)>(z))?2:0))\
00133                          + (((y)<-(z))?4:(((y)>(z))?8:0))
00134 pointer VPCLIP(ctx,n,argv)
00135 register context *ctx;
00136 int n;
00137 pointer argv[];
00138 {
00139   pointer v1, v2;
00140   pointer work;
00141   eusfloat_t x1,y1,z1, x2,y2,z2, x,y,z, temp;
00142   int c1, c2, c;
00143   numunion nu;
00144 
00145   ckarg(2);
00146   v1=argv[0]; v2=argv[1];
00147   if (!isfltvector(v1) || !isfltvector(v2)) error(E_FLOATVECTOR,NULL);
00148   x1=v1->c.fvec.fv[0]; y1=v1->c.fvec.fv[1]; z1=v1->c.fvec.fv[2];
00149   x2=v2->c.fvec.fv[0]; y2=v2->c.fvec.fv[1]; z2=v2->c.fvec.fv[2];
00150   c1 = clipcode(x1,y1,z1); c2 = clipcode(x2,y2,z2);
00151   while (c1 || c2 ) {
00152     if (c1 & c2) return(NIL);
00153     c = c1?c1:c2;
00154     if (c & 1) {
00155       temp = (z1+x1)/((x1-x2)-(z2-z1));
00156       z = temp*(z2-z1)+z1; x = -z; y = temp*(y2-y1) + y1;}
00157     else if (c & 2) {
00158       temp = (z1 - x1) / ((x2-x1) - (z2-z1));
00159       z = temp*(z2-z1) + z1; x = z; y = temp*(y2-y1) + y1;}
00160     else if (c & 4) {
00161       temp = (z1+y1) / ((y1-y2) - (z2-z1));
00162       z = temp*(z2-z1) + z1; x = temp*(x2-x1) + x1; y = -z;}
00163     else if (c & 8) {
00164       temp = (z1-y1) / ((y2 - y1) - (z2-z1));
00165       z = temp*(z2-z1) + z1; x = temp*(x2-x1) + x1; y = z;}
00166     if (c == c1) {
00167       x1 = x; y1 = y; z1 = z; c1 = clipcode(x,y,z);}
00168     else {
00169       x2 = x; y2 = y; z2 = z; c2 = clipcode(x,y,z);}
00170     }
00171   v1 = makefvector(3);
00172   v1->c.fvec.fv[0]=x1; v1->c.fvec.fv[1]=y1; v1->c.fvec.fv[2]=z1;
00173   vpush(v1);
00174   v2 = makefvector(3);
00175   v2->c.fvec.fv[0]=x2; v2->c.fvec.fv[1]=y2; v2->c.fvec.fv[2]=z2;
00176   work = cons(ctx,v2, NIL);
00177   work = cons(ctx,vpop(), work);
00178   return(work);
00179 }
00180 
00181 static windowcoords(x,y,z,w,wc)
00182 eusfloat_t x,y,z,w,wc[];
00183 { register int i,c,code;
00184   wc[0]=w+x; wc[1]=w-x; wc[2]=w+y; wc[3]=w-y; wc[4]=z; wc[5]=w-z;
00185   code=0; c=1; i=0;
00186   while (i<6) {
00187     if (wc[i++]<0) code += c;
00188     c <<= 1;}
00189   return(code);}
00190 
00191 pointer HOMO_VPCLIP(ctx,n,argv)
00192 register context *ctx;
00193 int n;
00194 pointer argv[];
00195 { pointer v1,v2,work;
00196   eusfloat_t v[4];
00197   eusfloat_t x1,y1,z1,w1, x2,y2,z2,w2, dx,dy,dz,dw, t1,t2,tt;
00198   register int i,c1,c2;
00199   eusfloat_t wc1[6],wc2[6];
00200 
00201   ckarg(2);
00202   v1=argv[0]; v2=argv[1];
00203   if (!isfltvector(v1) || !isfltvector(v2)) error(E_FLOATVECTOR,NULL);
00204   x1=v1->c.fvec.fv[0]; y1=v1->c.fvec.fv[1]; z1=v1->c.fvec.fv[2];
00205   if (vecsize(v1)>=4) w1=v1->c.fvec.fv[3]; else w1=1.0;
00206   x2=v2->c.fvec.fv[0]; y2=v2->c.fvec.fv[1]; z2=v2->c.fvec.fv[2];
00207   if (vecsize(v2)>=4) w2=v2->c.fvec.fv[3]; else w2=1.0;
00208 
00209   c1 = windowcoords(x1,y1,z1,w1,wc1); c2 = windowcoords(x2,y2,z2,w2,wc2);
00210   if ((c1 & c2)>0) return(NIL);
00211   t1=0.0; t2=1.0;
00212   for (i=0; i<6; i++)
00213     if ((wc1[i]<0) || (wc2[i]<0)) {
00214       tt = wc1[i] / (wc1[i]-wc2[i]);
00215       if (wc1[i]<0) {if (tt>t1) t1=tt;}
00216       else {if (tt<t2) t2=tt;}}
00217   if (t2 >= t1) {
00218     dx = x2-x1; dy = y2-y1; dz = z2-z1; dw = w2-w1;
00219     if (t2 != 1.0) {
00220       x2 = x1+t2*dx; y2 = y1+t2*dy; z2 = z1+t2*dz; w2 = w1+t2*dw;}
00221     if (t1 != 0.0) {
00222       x1 = x1+t1*dx; y1 = y1+t1*dy; z1 = z1+t1*dz; w1 = w1+t1*dw;}
00223 
00224     v1 = makefvector(4);
00225     v1->c.fvec.fv[0]=x1; v1->c.fvec.fv[1]=y1; v1->c.fvec.fv[2]=z1;
00226                                               v1->c.fvec.fv[3]=w1;
00227     vpush(v1);
00228     v2 = makefvector(4);
00229     v2->c.fvec.fv[0]=x2; v2->c.fvec.fv[1]=y2; v2->c.fvec.fv[2]=z2;
00230                                               v2->c.fvec.fv[3]=w2;
00231     work = cons(ctx,v2, NIL);
00232     work = cons(ctx,vpop(), work);
00233     return(work);    }
00234   else return(NIL);}
00235 
00236 pointer HOMO2NORMAL(ctx,n,argv)
00237 register context *ctx;
00238 register int n;
00239 pointer argv[];
00240 { register pointer a=argv[0],r;
00241   register int size;
00242   eusfloat_t w;
00243   ckarg2(1,2);
00244   if (!isfltvector(a)) error(E_FLOATVECTOR,NULL);
00245   size=vecsize(a);
00246   if (n==2) r=argv[1];
00247   else r=makefvector(size-1);
00248   w=a->c.fvec.fv[size-1];
00249   for (n=0; n<size-1; n++) r->c.fvec.fv[n]=a->c.fvec.fv[n]/w;
00250   if (vecsize(r)>n)  r->c.fvec.fv[size-1]=1.0;
00251   r->c.fvec.length=makeint(size-1);
00252   return(r);}
00253 
00254 pointer HOMOGENIZE(ctx,n,argv)
00255 register context *ctx;
00256 int n;
00257 pointer argv[];
00258 { register pointer a=argv[0],r;
00259   register int i,size;
00260   eusfloat_t w;
00261   numunion nu;
00262 
00263   ckarg2(1,2);
00264   if (!isfltvector(a)) error(E_FLOATVECTOR,NULL);
00265   if (n==2) {
00266     r=argv[1];
00267     if  (!isfltvector(r)) error(E_FLOATVECTOR,NULL);
00268     size=vecsize(a);
00269     if (size != vecsize(r)-1) error(E_ARRAYDIMENSION,NULL);}
00270   else {
00271     size=vecsize(a);
00272     r=makefvector(size+1);}
00273   for (i=0; i<size; i++) r->c.fvec.fv[i]=a->c.fvec.fv[i];
00274   r->c.fvec.fv[size]=1.0;
00275   return(r);}
00276     
00277 pointer intersection(ctx, n, argv)
00278 register context *ctx;
00279 int n;
00280 pointer argv[];
00281 {
00282   pointer mod, x;
00283 
00284   mod=argv[0];
00285 
00286 /*
00287   printf("intersection is being initialized %x\n", intersection);
00288   printf("ctx=%x mod=%x n=%d\n", ctx, mod, n);
00289   printf("LINEINTERSECTION=%x\n", LINEINTERSECTION);
00290   printf("defun=%x\n", defun);
00291   printf("compfun=%x\n", compfun);
00292 */
00293 
00294   x=defun(ctx,"LINE-INTERSECTION",mod,LINEINTERSECTION);
00295   defun(ctx,"LINE-INTERSECTION3",mod,LINEINTERSECTION3);
00296   /* clippers*/
00297   defun(ctx,"VIEWPORTCLIP",mod,VPCLIP);
00298   defun(ctx,"HOMO-VIEWPORT-CLIP",mod,HOMO_VPCLIP);
00299   defun(ctx,"HOMO2NORMAL",mod,HOMO2NORMAL); 
00300   defun(ctx,"HOMOGENIZE",mod,HOMOGENIZE);
00301   return(T); }
00302 


euslisp
Author(s): Toshihiro Matsui
autogenerated on Thu Mar 9 2017 04:57:49