00001
00002
00003
00004
00005
00006
00007
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);
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
00050 return(cons(ctx,up,cons(ctx,vp,NIL))); } }
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
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
00123
00124
00125
00126
00127
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
00288
00289
00290
00291
00292
00293
00294 x=defun(ctx,"LINE-INTERSECTION",mod,LINEINTERSECTION);
00295 defun(ctx,"LINE-INTERSECTION3",mod,LINEINTERSECTION3);
00296
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