intersection.c
Go to the documentation of this file.
1 /* intersection.c
2  finds an intersection point of two lines
3  (c)1987, MATSUI T., ETL
4 
5  1998--
6  Since this file is compiled and linked in the libeusgeo.so library,
7  this must be compiled with the -fpic (position independent) option.
8 
9 */
10 
11 #include "../c/eus.h"
12 #pragma init (init_object_module)
13 extern pointer intersection();
14 
15 static void init_object_module()
16  { add_module_initializer("intersection", intersection);}
17 
18 pointer LINEINTERSECTION(ctx,n,argv)
19 register context *ctx;
20 int n;
21 register pointer argv[];
22 {
23  eusfloat_t cz,u,v;
24  eusfloat_t a1x,a1y,b1x,b1y;
25  register eusfloat_t ax,ay,bx,by,abx,aby;
26  int range_check;
27  pointer up,vp;
28  numunion nu;
29 
30  ckarg2(4,5);
31  if (!isfltvector(argv[0])) error(E_FLOATVECTOR,NULL);
32  if (!isfltvector(argv[1])) error(E_FLOATVECTOR,NULL);
33  if (!isfltvector(argv[2])) error(E_FLOATVECTOR,NULL);
34  if (!isfltvector(argv[3])) error(E_FLOATVECTOR,NULL);
35  if (n>4 && (argv[4]!=NIL)) range_check=1; else range_check=0;
36  a1x=argv[0]->c.fvec.fv[0]; a1y=argv[0]->c.fvec.fv[1];
37  b1x=argv[2]->c.fvec.fv[0]; b1y=argv[2]->c.fvec.fv[1];
38  ax=argv[1]->c.fvec.fv[0]-a1x; ay=argv[1]->c.fvec.fv[1]-a1y;
39  bx=argv[3]->c.fvec.fv[0]-b1x; by=argv[3]->c.fvec.fv[1]-b1y;
40  abx=b1x-a1x; aby=b1y-a1y;
41  cz=ax*by-ay*bx;
42  if (cz==0) return(NIL); /* parallel Nov.1991*/
43  u=(abx*by-aby*bx)/cz;
44  v=(abx*ay-aby*ax)/cz;
45  if (range_check &&
46  (u<0.0 || 1.0<u || v<0.0 || 1.0<v)) return(NIL);
47  else {
48  up=makeflt(u); vp=makeflt(v);
49  /* printf("%f %f\n",u,v); */
50  return(cons(ctx,up,cons(ctx,vp,NIL))); } }
51 
52 /* (defun det3 (a b c)
53 /* %(a[0] * b[1] * c[2] - a[2] * b[1] * c[0] +
54 /* a[1] * b[2] * c[0] - a[1] * b[0] * c[2] +
55 /* a[2] * b[0] * c[1] - a[0] * b[2] * c[1]))
56 /*
57 /* (defun line-intersection3 (a1 a2 b1 b2 &optional tol)
58 /* (let* ((p1 a1) (v1 (normalize-vector (v- a2 a1)))
59 /* (p2 b1) (v2 (normalize-vector (v- b2 b1)))
60 /* (p2-p1 (v- p2 p1))
61 /* (cross (v* v1 v2))
62 /* (cross2 (v. cross cross))
63 /* t1 t2)
64 /* (cond ((< cross2 0.001) nil)
65 /* (t (list (/ (det3 p2-p1 v2 cross) cross2)
66 /* (/ (det3 p2-p1 v1 cross) cross2))))) )
67 */
68 
69 
70 
71 static eusfloat_t determinant3(a,b,c)
72 eusfloat_t *a, *b, *c;
73 { return(a[0] * b[1] * c[2] - a[2] * b[1] * c[0] +
74  a[1] * b[2] * c[0] - a[1] * b[0] * c[2] +
75  a[2] * b[0] * c[1] - a[0] * b[2] * c[1]);}
76 
77 static eusfloat_t *crossproduct(a,b,r)
78 register eusfloat_t *a, *b, *r;
79 { r[0]=a[1] * b[2] - a[2] * b[1];
80  r[1]=a[2] * b[0] - a[0] * b[2];
81  r[2]=a[0] * b[1] - a[1] * b[0];
82  return(r);}
83 
84 
85 pointer LINEINTERSECTION3(ctx,n,argv)
86 register context *ctx;
87 int n;
88 register pointer argv[];
89 { register eusfloat_t *fv;
90  eusfloat_t tolerance;
91  eusfloat_t cz,u,v;
92  eusfloat_t *p1, v1[3], *p2, v2[3], p2p1[3];
93  eusfloat_t cross[3], cross2;
94  numunion nu;
95 
96  ckarg2(4,5);
97  if (!isfltvector(argv[0])) error(E_FLOATVECTOR,NULL);
98  if (!isfltvector(argv[1])) error(E_FLOATVECTOR,NULL);
99  if (!isfltvector(argv[2])) error(E_FLOATVECTOR,NULL);
100  if (!isfltvector(argv[3])) error(E_FLOATVECTOR,NULL);
101  if (n==5) tolerance=ckfltval(argv[4]); else tolerance=0.0;
102 
103  p1=argv[0]->c.fvec.fv;
104  fv=argv[1]->c.fvec.fv;
105  v1[0]=fv[0]-p1[0]; v1[1]=fv[1]-p1[1]; v1[2]=fv[2]-p1[2];
106 
107  p2=argv[2]->c.fvec.fv;
108  fv=argv[3]->c.fvec.fv;
109  v2[0]=fv[0]-p2[0]; v2[1]=fv[1]-p2[1]; v2[2]=fv[2]-p2[2];
110 
111  p2p1[0]=p2[0]-p1[0]; p2p1[1]=p2[1]-p1[1]; p2p1[2]=p2[2]-p1[2];
112  crossproduct(v1,v2,cross);
113  cross2=cross[0]*cross[0] + cross[1]*cross[1] + cross[2]*cross[2];
114  if (cross2<tolerance) return(NIL);
115 
116  u=determinant3(p2p1,v2,cross)/cross2;
117  v=determinant3(p2p1,v1,cross)/cross2;
118  return(cons(ctx,makeflt(u),cons(ctx,makeflt(v),NIL))); }
119 
120 
121 /*
122 /* viewport clipping functions
123 /* VPCLIP is for normal coordinate system
124 /* HOMO_VPCLIP is for homogeneous coordinate system
125 /*
126 /* rewritten in 1987-Jun
127 /* Copyright(1986) Toshihiro MATSUI
128 */
129 
130 extern pointer makefvector();
131 
132 #define clipcode(x,y,z) (((x)<-(z))?1:(((x)>(z))?2:0))\
133  + (((y)<-(z))?4:(((y)>(z))?8:0))
134 pointer VPCLIP(ctx,n,argv)
135 register context *ctx;
136 int n;
137 pointer argv[];
138 {
139  pointer v1, v2;
140  pointer work;
141  eusfloat_t x1,y1,z1, x2,y2,z2, x,y,z, temp;
142  int c1, c2, c;
143  numunion nu;
144 
145  ckarg(2);
146  v1=argv[0]; v2=argv[1];
147  if (!isfltvector(v1) || !isfltvector(v2)) error(E_FLOATVECTOR,NULL);
148  x1=v1->c.fvec.fv[0]; y1=v1->c.fvec.fv[1]; z1=v1->c.fvec.fv[2];
149  x2=v2->c.fvec.fv[0]; y2=v2->c.fvec.fv[1]; z2=v2->c.fvec.fv[2];
150  c1 = clipcode(x1,y1,z1); c2 = clipcode(x2,y2,z2);
151  while (c1 || c2 ) {
152  if (c1 & c2) return(NIL);
153  c = c1?c1:c2;
154  if (c & 1) {
155  temp = (z1+x1)/((x1-x2)-(z2-z1));
156  z = temp*(z2-z1)+z1; x = -z; y = temp*(y2-y1) + y1;}
157  else if (c & 2) {
158  temp = (z1 - x1) / ((x2-x1) - (z2-z1));
159  z = temp*(z2-z1) + z1; x = z; y = temp*(y2-y1) + y1;}
160  else if (c & 4) {
161  temp = (z1+y1) / ((y1-y2) - (z2-z1));
162  z = temp*(z2-z1) + z1; x = temp*(x2-x1) + x1; y = -z;}
163  else if (c & 8) {
164  temp = (z1-y1) / ((y2 - y1) - (z2-z1));
165  z = temp*(z2-z1) + z1; x = temp*(x2-x1) + x1; y = z;}
166  if (c == c1) {
167  x1 = x; y1 = y; z1 = z; c1 = clipcode(x,y,z);}
168  else {
169  x2 = x; y2 = y; z2 = z; c2 = clipcode(x,y,z);}
170  }
171  v1 = makefvector(3);
172  v1->c.fvec.fv[0]=x1; v1->c.fvec.fv[1]=y1; v1->c.fvec.fv[2]=z1;
173  vpush(v1);
174  v2 = makefvector(3);
175  v2->c.fvec.fv[0]=x2; v2->c.fvec.fv[1]=y2; v2->c.fvec.fv[2]=z2;
176  work = cons(ctx,v2, NIL);
177  work = cons(ctx,vpop(), work);
178  return(work);
179 }
180 
181 static int windowcoords(x,y,z,w,wc)
182 eusfloat_t x,y,z,w,wc[];
183 { register int i,c,code;
184  wc[0]=w+x; wc[1]=w-x; wc[2]=w+y; wc[3]=w-y; wc[4]=z; wc[5]=w-z;
185  code=0; c=1; i=0;
186  while (i<6) {
187  if (wc[i++]<0) code += c;
188  c <<= 1;}
189  return(code);}
190 
191 pointer HOMO_VPCLIP(ctx,n,argv)
192 register context *ctx;
193 int n;
194 pointer argv[];
195 { pointer v1,v2,work;
196  eusfloat_t v[4];
197  eusfloat_t x1,y1,z1,w1, x2,y2,z2,w2, dx,dy,dz,dw, t1,t2,tt;
198  register int i,c1,c2;
199  eusfloat_t wc1[6],wc2[6];
200 
201  ckarg(2);
202  v1=argv[0]; v2=argv[1];
203  if (!isfltvector(v1) || !isfltvector(v2)) error(E_FLOATVECTOR,NULL);
204  x1=v1->c.fvec.fv[0]; y1=v1->c.fvec.fv[1]; z1=v1->c.fvec.fv[2];
205  if (vecsize(v1)>=4) w1=v1->c.fvec.fv[3]; else w1=1.0;
206  x2=v2->c.fvec.fv[0]; y2=v2->c.fvec.fv[1]; z2=v2->c.fvec.fv[2];
207  if (vecsize(v2)>=4) w2=v2->c.fvec.fv[3]; else w2=1.0;
208 
209  c1 = windowcoords(x1,y1,z1,w1,wc1); c2 = windowcoords(x2,y2,z2,w2,wc2);
210  if ((c1 & c2)>0) return(NIL);
211  t1=0.0; t2=1.0;
212  for (i=0; i<6; i++)
213  if ((wc1[i]<0) || (wc2[i]<0)) {
214  tt = wc1[i] / (wc1[i]-wc2[i]);
215  if (wc1[i]<0) {if (tt>t1) t1=tt;}
216  else {if (tt<t2) t2=tt;}}
217  if (t2 >= t1) {
218  dx = x2-x1; dy = y2-y1; dz = z2-z1; dw = w2-w1;
219  if (t2 != 1.0) {
220  x2 = x1+t2*dx; y2 = y1+t2*dy; z2 = z1+t2*dz; w2 = w1+t2*dw;}
221  if (t1 != 0.0) {
222  x1 = x1+t1*dx; y1 = y1+t1*dy; z1 = z1+t1*dz; w1 = w1+t1*dw;}
223 
224  v1 = makefvector(4);
225  v1->c.fvec.fv[0]=x1; v1->c.fvec.fv[1]=y1; v1->c.fvec.fv[2]=z1;
226  v1->c.fvec.fv[3]=w1;
227  vpush(v1);
228  v2 = makefvector(4);
229  v2->c.fvec.fv[0]=x2; v2->c.fvec.fv[1]=y2; v2->c.fvec.fv[2]=z2;
230  v2->c.fvec.fv[3]=w2;
231  work = cons(ctx,v2, NIL);
232  work = cons(ctx,vpop(), work);
233  return(work); }
234  else return(NIL);}
235 
236 pointer HOMO2NORMAL(ctx,n,argv)
237 register context *ctx;
238 register int n;
239 pointer argv[];
240 { register pointer a=argv[0],r;
241  register int size;
242  eusfloat_t w;
243  ckarg2(1,2);
244  if (!isfltvector(a)) error(E_FLOATVECTOR,NULL);
245  size=vecsize(a);
246  if (n==2) r=argv[1];
247  else r=makefvector(size-1);
248  w=a->c.fvec.fv[size-1];
249  for (n=0; n<size-1; n++) r->c.fvec.fv[n]=a->c.fvec.fv[n]/w;
250  if (vecsize(r)>n) r->c.fvec.fv[size-1]=1.0;
251  r->c.fvec.length=makeint(size-1);
252  return(r);}
253 
254 pointer HOMOGENIZE(ctx,n,argv)
255 register context *ctx;
256 int n;
257 pointer argv[];
258 { register pointer a=argv[0],r;
259  register int i,size;
260  eusfloat_t w;
261  numunion nu;
262 
263  ckarg2(1,2);
264  if (!isfltvector(a)) error(E_FLOATVECTOR,NULL);
265  if (n==2) {
266  r=argv[1];
267  if (!isfltvector(r)) error(E_FLOATVECTOR,NULL);
268  size=vecsize(a);
269  if (size != vecsize(r)-1) error(E_ARRAYDIMENSION,NULL);}
270  else {
271  size=vecsize(a);
272  r=makefvector(size+1);}
273  for (i=0; i<size; i++) r->c.fvec.fv[i]=a->c.fvec.fv[i];
274  r->c.fvec.fv[size]=1.0;
275  return(r);}
276 
277 pointer intersection(ctx, n, argv)
278 register context *ctx;
279 int n;
280 pointer argv[];
281 {
282  pointer mod, x;
283 
284  mod=argv[0];
285 
286 /*
287  printf("intersection is being initialized %x\n", intersection);
288  printf("ctx=%x mod=%x n=%d\n", ctx, mod, n);
289  printf("LINEINTERSECTION=%x\n", LINEINTERSECTION);
290  printf("defun=%x\n", defun);
291  printf("compfun=%x\n", compfun);
292 */
293 
294  x=defun(ctx,"LINE-INTERSECTION",mod,LINEINTERSECTION,NULL);
295  defun(ctx,"LINE-INTERSECTION3",mod,LINEINTERSECTION3,NULL);
296  /* clippers*/
297  defun(ctx,"VIEWPORTCLIP",mod,VPCLIP,NULL);
298  defun(ctx,"HOMO-VIEWPORT-CLIP",mod,HOMO_VPCLIP,NULL);
299  defun(ctx,"HOMO2NORMAL",mod,HOMO2NORMAL,NULL);
300  defun(ctx,"HOMOGENIZE",mod,HOMOGENIZE,NULL);
301  return(T); }
302 
HOMO2NORMAL
pointer HOMO2NORMAL(context *ctx, int n, argv)
Definition: intersection.c:236
HOMO_VPCLIP
pointer HOMO_VPCLIP(context *ctx, int n, argv)
Definition: intersection.c:191
makeint
#define makeint(v)
Definition: sfttest.c:2
VPCLIP
pointer VPCLIP(context *ctx, int n, argv)
Definition: intersection.c:134
context
Definition: eus.h:522
cons
pointer cons(context *, pointer, pointer)
Definition: makes.c:97
clipcode
#define clipcode(x, y, z)
Definition: intersection.c:132
windowcoords
static int windowcoords(eusfloat_t x, eusfloat_t y, eusfloat_t z, eusfloat_t w, wc)
Definition: intersection.c:181
T
pointer T
Definition: eus.c:110
n
GLfloat n[6][3]
Definition: cube.c:15
crossproduct
static eusfloat_t * crossproduct(eusfloat_t *a, eusfloat_t *b, eusfloat_t *r)
Definition: intersection.c:77
HOMOGENIZE
pointer HOMOGENIZE(context *ctx, int n, argv)
Definition: intersection.c:254
defun
defun("ADR_TO_STRING", mod, ADR_TO_STRING)
ckarg
ckarg(2)
add_module_initializer
void add_module_initializer(char *, pointer(*)())
Definition: loadelf.c:86
ckfltval
float ckfltval()
cell::c
union cell::cellunion c
determinant3
static eusfloat_t determinant3(eusfloat_t *a, eusfloat_t *b, eusfloat_t *c)
Definition: intersection.c:71
makefvector
pointer makefvector()
numunion
Definition: eus.h:426
cell
Definition: eus.h:379
error
pointer error(enum errorcode ec,...) pointer error(va_alist) va_dcl
Definition: eus.c:297
code
Definition: eus.h:228
intersection
pointer intersection()
LINEINTERSECTION3
pointer LINEINTERSECTION3(context *ctx, int n, argv)
Definition: intersection.c:85
NULL
#define NULL
Definition: transargv.c:8
v
GLfloat v[8][3]
Definition: cube.c:21
floatvector::fv
eusfloat_t fv[1]
Definition: eus.h:307
eusfloat_t
double eusfloat_t
Definition: eus.h:20
NIL
pointer NIL
Definition: eus.c:110
LINEINTERSECTION
pointer LINEINTERSECTION(context *ctx, int n, argv)
Definition: intersection.c:18
if
if(n==1)
Definition: unixcall.c:491
init_object_module
static void init_object_module()
Definition: intersection.c:15
a
char a[26]
Definition: freq.c:4
cell::cellunion::fvec
struct floatvector fvec
Definition: eus.h:413
makeflt
pointer makeflt()

euslisp
Author(s): Toshihiro Matsui
autogenerated on Fri Feb 21 2020 03:20:54