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  pointer up,vp;
95  numunion nu;
96 
97  ckarg2(4,5);
98  if (!isfltvector(argv[0])) error(E_FLOATVECTOR,NULL);
99  if (!isfltvector(argv[1])) error(E_FLOATVECTOR,NULL);
100  if (!isfltvector(argv[2])) error(E_FLOATVECTOR,NULL);
101  if (!isfltvector(argv[3])) error(E_FLOATVECTOR,NULL);
102  if (n==5) tolerance=ckfltval(argv[4]); else tolerance=0.0;
103 
104  p1=argv[0]->c.fvec.fv;
105  fv=argv[1]->c.fvec.fv;
106  v1[0]=fv[0]-p1[0]; v1[1]=fv[1]-p1[1]; v1[2]=fv[2]-p1[2];
107 
108  p2=argv[2]->c.fvec.fv;
109  fv=argv[3]->c.fvec.fv;
110  v2[0]=fv[0]-p2[0]; v2[1]=fv[1]-p2[1]; v2[2]=fv[2]-p2[2];
111 
112  p2p1[0]=p2[0]-p1[0]; p2p1[1]=p2[1]-p1[1]; p2p1[2]=p2[2]-p1[2];
113  crossproduct(v1,v2,cross);
114  cross2=cross[0]*cross[0] + cross[1]*cross[1] + cross[2]*cross[2];
115  if (cross2<tolerance) return(NIL);
116 
117  u=determinant3(p2p1,v2,cross)/cross2;
118  v=determinant3(p2p1,v1,cross)/cross2;
119  up=makeflt(u); vp=makeflt(v);
120  return(cons(ctx,up,cons(ctx,vp,NIL))); }
121 
122 
123 /*
124 /* viewport clipping functions
125 /* VPCLIP is for normal coordinate system
126 /* HOMO_VPCLIP is for homogeneous coordinate system
127 /*
128 /* rewritten in 1987-Jun
129 /* Copyright(1986) Toshihiro MATSUI
130 */
131 
132 extern pointer makefvector();
133 
134 #define clipcode(x,y,z) (((x)<-(z))?1:(((x)>(z))?2:0))\
135  + (((y)<-(z))?4:(((y)>(z))?8:0))
136 pointer VPCLIP(ctx,n,argv)
137 register context *ctx;
138 int n;
139 pointer argv[];
140 {
141  pointer v1, v2;
142  pointer work;
143  eusfloat_t x1,y1,z1, x2,y2,z2, x,y,z, temp;
144  int c1, c2, c;
145  numunion nu;
146 
147  ckarg(2);
148  v1=argv[0]; v2=argv[1];
149  if (!isfltvector(v1) || !isfltvector(v2)) error(E_FLOATVECTOR,NULL);
150  x1=v1->c.fvec.fv[0]; y1=v1->c.fvec.fv[1]; z1=v1->c.fvec.fv[2];
151  x2=v2->c.fvec.fv[0]; y2=v2->c.fvec.fv[1]; z2=v2->c.fvec.fv[2];
152  c1 = clipcode(x1,y1,z1); c2 = clipcode(x2,y2,z2);
153  while (c1 || c2 ) {
154  if (c1 & c2) return(NIL);
155  c = c1?c1:c2;
156  if (c & 1) {
157  temp = (z1+x1)/((x1-x2)-(z2-z1));
158  z = temp*(z2-z1)+z1; x = -z; y = temp*(y2-y1) + y1;}
159  else if (c & 2) {
160  temp = (z1 - x1) / ((x2-x1) - (z2-z1));
161  z = temp*(z2-z1) + z1; x = z; y = temp*(y2-y1) + y1;}
162  else if (c & 4) {
163  temp = (z1+y1) / ((y1-y2) - (z2-z1));
164  z = temp*(z2-z1) + z1; x = temp*(x2-x1) + x1; y = -z;}
165  else if (c & 8) {
166  temp = (z1-y1) / ((y2 - y1) - (z2-z1));
167  z = temp*(z2-z1) + z1; x = temp*(x2-x1) + x1; y = z;}
168  if (c == c1) {
169  x1 = x; y1 = y; z1 = z; c1 = clipcode(x,y,z);}
170  else {
171  x2 = x; y2 = y; z2 = z; c2 = clipcode(x,y,z);}
172  }
173  v1 = makefvector(3);
174  v1->c.fvec.fv[0]=x1; v1->c.fvec.fv[1]=y1; v1->c.fvec.fv[2]=z1;
175  vpush(v1);
176  v2 = makefvector(3);
177  v2->c.fvec.fv[0]=x2; v2->c.fvec.fv[1]=y2; v2->c.fvec.fv[2]=z2;
178  work = cons(ctx,v2, NIL);
179  work = cons(ctx,vpop(), work);
180  return(work);
181 }
182 
183 static int windowcoords(x,y,z,w,wc)
184 eusfloat_t x,y,z,w,wc[];
185 { register int i,c,code;
186  wc[0]=w+x; wc[1]=w-x; wc[2]=w+y; wc[3]=w-y; wc[4]=z; wc[5]=w-z;
187  code=0; c=1; i=0;
188  while (i<6) {
189  if (wc[i++]<0) code += c;
190  c <<= 1;}
191  return(code);}
192 
193 pointer HOMO_VPCLIP(ctx,n,argv)
194 register context *ctx;
195 int n;
196 pointer argv[];
197 { pointer v1,v2,work;
198  eusfloat_t v[4];
199  eusfloat_t x1,y1,z1,w1, x2,y2,z2,w2, dx,dy,dz,dw, t1,t2,tt;
200  register int i,c1,c2;
201  eusfloat_t wc1[6],wc2[6];
202 
203  ckarg(2);
204  v1=argv[0]; v2=argv[1];
205  if (!isfltvector(v1) || !isfltvector(v2)) error(E_FLOATVECTOR,NULL);
206  x1=v1->c.fvec.fv[0]; y1=v1->c.fvec.fv[1]; z1=v1->c.fvec.fv[2];
207  if (vecsize(v1)>=4) w1=v1->c.fvec.fv[3]; else w1=1.0;
208  x2=v2->c.fvec.fv[0]; y2=v2->c.fvec.fv[1]; z2=v2->c.fvec.fv[2];
209  if (vecsize(v2)>=4) w2=v2->c.fvec.fv[3]; else w2=1.0;
210 
211  c1 = windowcoords(x1,y1,z1,w1,wc1); c2 = windowcoords(x2,y2,z2,w2,wc2);
212  if ((c1 & c2)>0) return(NIL);
213  t1=0.0; t2=1.0;
214  for (i=0; i<6; i++)
215  if ((wc1[i]<0) || (wc2[i]<0)) {
216  tt = wc1[i] / (wc1[i]-wc2[i]);
217  if (wc1[i]<0) {if (tt>t1) t1=tt;}
218  else {if (tt<t2) t2=tt;}}
219  if (t2 >= t1) {
220  dx = x2-x1; dy = y2-y1; dz = z2-z1; dw = w2-w1;
221  if (t2 != 1.0) {
222  x2 = x1+t2*dx; y2 = y1+t2*dy; z2 = z1+t2*dz; w2 = w1+t2*dw;}
223  if (t1 != 0.0) {
224  x1 = x1+t1*dx; y1 = y1+t1*dy; z1 = z1+t1*dz; w1 = w1+t1*dw;}
225 
226  v1 = makefvector(4);
227  v1->c.fvec.fv[0]=x1; v1->c.fvec.fv[1]=y1; v1->c.fvec.fv[2]=z1;
228  v1->c.fvec.fv[3]=w1;
229  vpush(v1);
230  v2 = makefvector(4);
231  v2->c.fvec.fv[0]=x2; v2->c.fvec.fv[1]=y2; v2->c.fvec.fv[2]=z2;
232  v2->c.fvec.fv[3]=w2;
233  work = cons(ctx,v2, NIL);
234  work = cons(ctx,vpop(), work);
235  return(work); }
236  else return(NIL);}
237 
238 pointer HOMO2NORMAL(ctx,n,argv)
239 register context *ctx;
240 register int n;
241 pointer argv[];
242 { register pointer a=argv[0],r;
243  register int size;
244  eusfloat_t w;
245  ckarg2(1,2);
246  if (!isfltvector(a)) error(E_FLOATVECTOR,NULL);
247  size=vecsize(a);
248  if (n==2) r=argv[1];
249  else r=makefvector(size-1);
250  w=a->c.fvec.fv[size-1];
251  for (n=0; n<size-1; n++) r->c.fvec.fv[n]=a->c.fvec.fv[n]/w;
252  if (vecsize(r)>n) r->c.fvec.fv[size-1]=1.0;
253  r->c.fvec.length=makeint(size-1);
254  return(r);}
255 
256 pointer HOMOGENIZE(ctx,n,argv)
257 register context *ctx;
258 int n;
259 pointer argv[];
260 { register pointer a=argv[0],r;
261  register int i,size;
262  eusfloat_t w;
263  numunion nu;
264 
265  ckarg2(1,2);
266  if (!isfltvector(a)) error(E_FLOATVECTOR,NULL);
267  if (n==2) {
268  r=argv[1];
269  if (!isfltvector(r)) error(E_FLOATVECTOR,NULL);
270  size=vecsize(a);
271  if (size != vecsize(r)-1) error(E_ARRAYDIMENSION,NULL);}
272  else {
273  size=vecsize(a);
274  r=makefvector(size+1);}
275  for (i=0; i<size; i++) r->c.fvec.fv[i]=a->c.fvec.fv[i];
276  r->c.fvec.fv[size]=1.0;
277  return(r);}
278 
279 pointer intersection(ctx, n, argv)
280 register context *ctx;
281 int n;
282 pointer argv[];
283 {
284  pointer mod, x;
285 
286  mod=argv[0];
287 
288 /*
289  printf("intersection is being initialized %x\n", intersection);
290  printf("ctx=%x mod=%x n=%d\n", ctx, mod, n);
291  printf("LINEINTERSECTION=%x\n", LINEINTERSECTION);
292  printf("defun=%x\n", defun);
293  printf("compfun=%x\n", compfun);
294 */
295 
296  x=defun(ctx,"LINE-INTERSECTION",mod,LINEINTERSECTION,NULL);
297  defun(ctx,"LINE-INTERSECTION3",mod,LINEINTERSECTION3,NULL);
298  /* clippers*/
299  defun(ctx,"VIEWPORTCLIP",mod,VPCLIP,NULL);
300  defun(ctx,"HOMO-VIEWPORT-CLIP",mod,HOMO_VPCLIP,NULL);
301  defun(ctx,"HOMO2NORMAL",mod,HOMO2NORMAL,NULL);
302  defun(ctx,"HOMOGENIZE",mod,HOMOGENIZE,NULL);
303  return(T); }
304 
HOMO2NORMAL
pointer HOMO2NORMAL(context *ctx, int n, argv)
Definition: intersection.c:238
HOMO_VPCLIP
pointer HOMO_VPCLIP(context *ctx, int n, argv)
Definition: intersection.c:193
makeint
#define makeint(v)
Definition: sfttest.c:2
VPCLIP
pointer VPCLIP(context *ctx, int n, argv)
Definition: intersection.c:136
context
Definition: eus.h:524
cons
pointer cons(context *, pointer, pointer)
Definition: makes.c:97
clipcode
#define clipcode(x, y, z)
Definition: intersection.c:134
windowcoords
static int windowcoords(eusfloat_t x, eusfloat_t y, eusfloat_t z, eusfloat_t w, wc)
Definition: intersection.c:183
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:256
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:428
cell
Definition: eus.h:381
error
pointer error(enum errorcode ec,...) pointer error(va_alist) va_dcl
Definition: eus.c:297
code
Definition: eus.h:230
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:309
eusfloat_t
double eusfloat_t
Definition: eus.h:21
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:492
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:415
makeflt
pointer makeflt()

euslisp
Author(s): Toshihiro Matsui
autogenerated on Mon Feb 28 2022 22:18:27