1 /* Steven Andrews, 2/17/06
2 Simple routines for 1D, 2D, and 3D geometry.
3 See documentation called Geometry_doc.doc and Geometry_doc.pdf.
4 Copyright 2006-2007 by Steven Andrews. This work is distributed under the terms
5 of the Gnu Lesser General Public License (LGPL). */
6
7 #include <float.h>
8 #include <math.h>
9 #include <stdio.h>
10 #include "Geometry.h"
11 #include "math2.h"
12
13 #define EPSILON 100*DBL_EPSILON
14
15 #ifndef INFINITY // INFINITY is defined in math.h, so should already be defined
16 #include <limits> // MSVC 2012
17 #define INFINITY std::numeric_limits<float>::infinity( );
18 #endif
19
20 /***************** ... Center **************/
21
Geo_LineCenter(double ** point,double * cent,int dim)22 void Geo_LineCenter(double **point,double *cent,int dim) {
23 int d;
24
25 for(d=0;d<dim;d++)
26 cent[d]=0.5*(point[0][d]+point[1][d]);
27 return; }
28
29
Geo_RectCenter(double ** point,double * cent,int dim)30 void Geo_RectCenter(double **point,double *cent,int dim) {
31 if(dim==1)
32 cent[0]=point[0][0];
33 else if(dim==2) {
34 cent[0]=0.5*(point[0][0]+point[1][0]);
35 cent[1]=0.5*(point[0][1]+point[1][1]); }
36 else if(dim==3) {
37 cent[0]=0.5*(point[0][0]+point[2][0]);
38 cent[1]=0.5*(point[0][1]+point[2][1]);
39 cent[2]=0.5*(point[0][2]+point[2][2]); }
40 return; }
41
42
Geo_TriCenter(double ** point,double * cent,int dim)43 void Geo_TriCenter(double **point,double *cent,int dim) {
44 if(dim==1)
45 cent[0]=point[0][0];
46 else if(dim==2) {
47 cent[0]=0.5*(point[0][0]+point[1][0]);
48 cent[1]=0.5*(point[0][1]+point[1][1]); }
49 else if(dim==3) {
50 cent[0]=(1.0/3.0)*(point[0][0]+point[1][0]+point[2][0]);
51 cent[1]=(1.0/3.0)*(point[0][1]+point[1][1]+point[2][1]);
52 cent[2]=(1.0/3.0)*(point[0][2]+point[1][2]+point[2][2]); }
53 return; }
54
55
56 /***************** ... Normal **************/
57
Geo_LineNormal(double * pt1,double * pt2,double * ans)58 double Geo_LineNormal(double *pt1,double *pt2,double *ans) {
59 double dx,dy,len,leninv;
60
61 dx=pt2[0]-pt1[0];
62 dy=pt2[1]-pt1[1];
63 len=sqrt(dx*dx+dy*dy);
64 if(len>0) {
65 leninv=1.0/len;
66 ans[0]=leninv*dy;
67 ans[1]=-leninv*dx; }
68 else {
69 ans[0]=1;
70 ans[1]=0; }
71 return len; }
72
73
Geo_LineNormal2D(double * pt1,double * pt2,double * point,double * ans)74 double Geo_LineNormal2D(double *pt1,double *pt2,double *point,double *ans) {
75 double dx,dy,dot,len;
76
77 dx=pt2[0]-pt1[0];
78 dy=pt2[1]-pt1[1];
79 len=sqrt(dx*dx+dy*dy);
80 if(len>EPSILON) {
81 ans[0]=dy/len;
82 ans[1]=-dx/len;
83 dot=(point[0]-pt1[0])*ans[0]+(point[1]-pt1[1])*ans[1];
84 if(dot<0) {
85 ans[0]*=-1;
86 ans[1]*=-1;
87 dot*=-1; }
88 return sqrt(dot); }
89
90 ans[0]=point[0]-pt1[0];
91 ans[1]=point[1]-pt1[1];
92 dot=ans[0]*ans[0]+ans[1]*ans[1];
93 if(dot<EPSILON) { // pt1 == pt2 and point == pt1 so return unit x
94 ans[0]=1;
95 ans[1]=0;
96 return 0; }
97 dot=sqrt(dot); // pt1 == pt2 so return normalized point
98 ans[0]/=dot;
99 ans[1]/=dot;
100 return dot; }
101
102
Geo_LineNormal3D(double * pt1,double * pt2,double * point,double * ans)103 double Geo_LineNormal3D(double *pt1,double *pt2,double *point,double *ans) {
104 double dot,line[3];
105
106 line[0]=pt2[0]-pt1[0];
107 line[1]=pt2[1]-pt1[1];
108 line[2]=pt2[2]-pt1[2];
109 dot=line[0]*line[0]+line[1]*line[1]+line[2]*line[2];
110 if(dot<EPSILON) { // pt1 == pt2
111 ans[0]=point[0]-pt1[0];
112 ans[1]=point[1]-pt1[1];
113 ans[2]=point[2]-pt1[2];
114 dot=ans[0]*ans[0]+ans[1]*ans[1]+ans[2]*ans[2];
115 if(dot<EPSILON) { // pt1 == pt2 and point == pt1 so return unit x
116 ans[0]=1;
117 ans[1]=0;
118 ans[2]=0;
119 return 0; }
120 dot=sqrt(dot); // pt1 == pt2 so return normalized point
121 ans[0]/=dot;
122 ans[1]/=dot;
123 ans[2]/=dot;
124 return dot; }
125 dot=sqrt(dot);
126 line[0]/=dot;
127 line[1]/=dot;
128 line[2]/=dot;
129
130 ans[0]=point[0]-pt1[0];
131 ans[1]=point[1]-pt1[1];
132 ans[2]=point[2]-pt1[2];
133 dot=ans[0]*line[0]+ans[1]*line[1]+ans[2]*line[2];
134 ans[0]-=dot*line[0];
135 ans[1]-=dot*line[1];
136 ans[2]-=dot*line[2];
137 dot=ans[0]*line[0]+ans[1]*line[1]+ans[2]*line[2];
138 ans[0]-=dot*line[0];
139 ans[1]-=dot*line[1];
140 ans[2]-=dot*line[2];
141 dot=ans[0]*ans[0]+ans[1]*ans[1]+ans[2]*ans[2];
142 if(dot<EPSILON) { // point is on line
143 if(line[0]==0 && line[1]==0) { // line parallel to z so return unit x
144 ans[0]=1;
145 ans[1]=0;
146 ans[2]=0;
147 return 0; }
148 ans[0]=line[1]; // return right side perpendicular in x,y plane
149 ans[1]=-line[0];
150 ans[2]=0;
151 dot=sqrt(ans[0]*ans[0]+ans[1]*ans[1]+ans[2]*ans[2]);
152 ans[0]/=dot;
153 ans[1]/=dot;
154 ans[2]/=dot;
155 return 0; }
156 dot=sqrt(dot);
157 ans[0]/=dot;
158 ans[1]/=dot;
159 ans[2]/=dot;
160 return dot; }
161
162
Geo_LineNormPos(double * pt1,double * pt2,double * point,int dim,double * distptr)163 double Geo_LineNormPos(double *pt1,double *pt2,double *point,int dim,double *distptr) {
164 double dot,len2,pointlen2;
165 int d;
166
167 len2=dot=pointlen2=0;
168 for(d=0;d<dim;d++) {
169 dot+=(point[d]-pt1[d])*(pt2[d]-pt1[d]);
170 len2+=(pt2[d]-pt1[d])*(pt2[d]-pt1[d]);
171 pointlen2+=(point[d]-pt1[d])*(point[d]-pt1[d]); }
172 if(distptr) *distptr=sqrt(pointlen2-dot*dot/len2);
173 return dot/len2; }
174
175
Geo_TriNormal(double * pt1,double * pt2,double * pt3,double * ans)176 double Geo_TriNormal(double *pt1,double *pt2,double *pt3,double *ans) {
177 double dx1,dy1,dz1,dx2,dy2,dz2,area;
178
179 dx1=pt2[0]-pt1[0];
180 dy1=pt2[1]-pt1[1];
181 dz1=pt2[2]-pt1[2];
182 dx2=pt3[0]-pt2[0];
183 dy2=pt3[1]-pt2[1];
184 dz2=pt3[2]-pt2[2];
185 ans[0]=dy1*dz2-dz1*dy2; // v1 cross v2
186 ans[1]=-dx1*dz2+dz1*dx2;
187 ans[2]=dx1*dy2-dy1*dx2;
188 area=sqrt(ans[0]*ans[0]+ans[1]*ans[1]+ans[2]*ans[2]);
189 if(area>EPSILON) {
190 ans[0]/=area;
191 ans[1]/=area;
192 ans[2]/=area; }
193 else {
194 Geo_LineNormal(pt1,pt2,ans);
195 ans[2]=0; }
196 return area/2; }
197
198
Geo_SphereNormal(double * cent,double * pt,int front,int dim,double * ans)199 double Geo_SphereNormal(double *cent,double *pt,int front,int dim,double *ans) {
200 int d;
201 double dist;
202
203 dist=0;
204 for(d=0;d<dim;d++) {
205 ans[d]=front*(pt[d]-cent[d]);
206 dist+=ans[d]*ans[d]; }
207 if(dist>0) {
208 dist=sqrt(dist);
209 for(d=0;d<dim;d++) ans[d]/=dist; }
210 else {
211 ans[0]=1;
212 for(d=1;d<dim;d++) ans[d]=0; }
213 return dist; }
214
215
216 /******** Unit Vectors *******/
217
218
Geo_UnitCross(double * v1start,double * v1end,double * v2start,double * v2end,double * ans)219 double Geo_UnitCross(double *v1start,double *v1end,double *v2start,double *v2end,double *ans) {
220 double ax,ay,az,bx,by,bz,cx,cy,cz,len;
221
222 if(v1start) {
223 ax=v1end[0]-v1start[0];
224 ay=v1end[1]-v1start[1];
225 az=v1end[2]-v1start[2]; }
226 else {
227 ax=v1end[0];
228 ay=v1end[1];
229 az=v1end[2]; }
230 if(v2start) {
231 bx=v2end[0]-v2start[0];
232 by=v2end[1]-v2start[1];
233 bz=v2end[2]-v2start[2]; }
234 else {
235 bx=v2end[0];
236 by=v2end[1];
237 bz=v2end[2]; }
238 cx=ay*bz-az*by;
239 cy=az*bx-ax*bz;
240 cz=ax*by-ay*bx;
241 len=sqrt(cx*cx+cy*cy+cz*cz);
242 if(len>EPSILON) {
243 ans[0]=cx/len;
244 ans[1]=cy/len;
245 ans[2]=cz/len;
246 return len; }
247 ans[0]=ans[1]=ans[2]=0;
248 return 0; }
249
250
Geo_TriUnitVects(double * pt1,double * pt2,double * pt3,double * unit0,double * unit1,double * unit2)251 double Geo_TriUnitVects(double *pt1,double *pt2,double *pt3,double *unit0,double *unit1,double *unit2) {
252 double area,dist;
253
254 area=Geo_TriNormal(pt1,pt2,pt3,unit0); // unit0 is triangle normal
255 dist=sqrt((pt2[0]-pt1[0])*(pt2[0]-pt1[0]) + (pt2[1]-pt1[1])*(pt2[1]-pt1[1]) + (pt2[2]-pt1[2])*(pt2[2]-pt1[2])); // distance pt1 to pt2
256 unit1[0]=(pt2[0]-pt1[0])/dist; // unit1 is line from pt1 to pt2
257 unit1[1]=(pt2[1]-pt1[1])/dist;
258 unit1[2]=(pt2[2]-pt1[2])/dist;
259 unit2[0]=unit0[1]*unit1[2]-unit0[2]*unit1[1]; // unit2 is cross product unit0 x unit1
260 unit2[1]=unit0[2]*unit1[0]-unit0[0]*unit1[2];
261 unit2[2]=unit0[0]*unit1[1]-unit0[1]*unit1[0];
262 return area; }
263
264
Geo_SphereUnitVects(double * cent,double * top,double * point,int front,double * unit0,double * unit1,double * unit2)265 double Geo_SphereUnitVects(double *cent,double *top,double *point,int front,double *unit0,double *unit1,double *unit2) {
266 double radius;
267
268 radius=Geo_SphereNormal(cent,point,front,3,unit0); // unit0 is sphere normal
269 Geo_LineNormal3D(cent,point,top,unit1); // unit1 is normal of normal line, pointing to sphere top
270 unit2[0]=unit0[1]*unit1[2]-unit0[2]*unit1[1]; // unit2 is cross product unit0 x unit1
271 unit2[1]=unit0[2]*unit1[0]-unit0[0]*unit1[2];
272 unit2[2]=unit0[0]*unit1[1]-unit0[1]*unit1[0];
273 return radius; }
274
275
Geo_CylUnitVects(double * pt1,double * pt2,double * point,double * unit0,double * unit1,double * unit2)276 double Geo_CylUnitVects(double *pt1,double *pt2,double *point,double *unit0,double *unit1,double *unit2) {
277 double radius,dist;
278
279 radius=Geo_LineNormal3D(pt1,pt2,point,unit0); // unit0 is cylinder normal
280 dist=sqrt((pt2[0]-pt1[0])*(pt2[0]-pt1[0]) + (pt2[1]-pt1[1])*(pt2[1]-pt1[1]) + (pt2[2]-pt1[2])*(pt2[2]-pt1[2])); // distance pt1 to pt2
281 unit1[0]=(pt2[0]-pt1[0])/dist; // unit1 is line from pt1 to pt2
282 unit1[1]=(pt2[1]-pt1[1])/dist;
283 unit1[2]=(pt2[2]-pt1[2])/dist;
284 unit2[0]=unit0[1]*unit1[2]-unit0[2]*unit1[1]; // unit2 is cross product unit0 x unit1
285 unit2[1]=unit0[2]*unit1[0]-unit0[0]*unit1[2];
286 unit2[2]=unit0[0]*unit1[1]-unit0[1]*unit1[0];
287 return radius; }
288
289
Geo_DiskUnitVects(double * cent,double * front,double * point,double * unit0,double * unit1,double * unit2)290 double Geo_DiskUnitVects(double *cent,double *front,double *point,double *unit0,double *unit1,double *unit2) {
291 double dist;
292
293 unit0[0]=front[0]; // unit0 is simply front vector
294 unit0[1]=front[1];
295 unit0[2]=front[2];
296 dist=sqrt((point[0]-cent[0])*(point[0]-cent[0]) + (point[1]-cent[1])*(point[1]-cent[1]) + (point[2]-cent[2])*(point[2]-cent[2])); // distance cent to point
297 unit1[0]=(point[0]-cent[0])/dist; // unit1 is radial line from cent to point
298 unit1[1]=(point[1]-cent[1])/dist;
299 unit1[2]=(point[2]-cent[2])/dist;
300 unit2[0]=unit0[1]*unit1[2]-unit0[2]*unit1[1]; // unit2 is cross product unit0 x unit1
301 unit2[1]=unit0[2]*unit1[0]-unit0[0]*unit1[2];
302 unit2[2]=unit0[0]*unit1[1]-unit0[1]*unit1[0];
303 return dist; }
304
305
306 /********* Area ***********/
307
Geo_LineLength(double * pt1,double * pt2,int dim)308 double Geo_LineLength(double *pt1,double *pt2,int dim) {
309 double len;
310 int d;
311
312 len=0;
313 for(d=0;d<dim;d++) len+=(pt2[d]-pt1[d])*(pt2[d]-pt1[d]);
314 return sqrt(len); }
315
316
Geo_TriArea2(double * pt1,double * pt2,double * pt3)317 double Geo_TriArea2(double *pt1,double *pt2,double *pt3) {
318 return -0.5*((pt2[1]-pt1[1])*(pt3[0]-pt1[0])-(pt2[0]-pt1[0])*(pt3[1]-pt1[1])); }
319
320
Geo_TriArea3D(double * pt1,double * pt2,double * pt3)321 double Geo_TriArea3D(double *pt1,double *pt2,double *pt3) {
322 double a,b,c;
323
324 a = sqrt((pt1[0]-pt2[0])*(pt1[0]-pt2[0]) + (pt1[1]-pt2[1])*(pt1[1]-pt2[1]) + (pt1[2]-pt2[2])*(pt1[2]-pt2[2])); //distance p1 to p2
325 b = sqrt((pt1[0]-pt3[0])*(pt1[0]-pt3[0]) + (pt1[1]-pt3[1])*(pt1[1]-pt3[1]) + (pt1[2]-pt3[2])*(pt1[2]-pt3[2])); //distance p1 to p3
326 c = sqrt((pt2[0]-pt3[0])*(pt2[0]-pt3[0]) + (pt2[1]-pt3[1])*(pt2[1]-pt3[1]) + (pt2[2]-pt3[2])*(pt2[2]-pt3[2])); //distance p2 to p3
327 return sqrt((a+(b+c))*(c-(a-b))*(c+(a-b))*(a+(b-c)))/4.0; }
328
329
Geo_TriArea3(double * pt1,double * pt2,double * pt3,double * norm)330 double Geo_TriArea3(double *pt1,double *pt2,double *pt3,double *norm) {
331 double base[3],cross[3];
332
333 base[0]=pt2[0]-pt1[0];
334 base[1]=pt2[1]-pt1[1];
335 base[2]=pt2[2]-pt1[2];
336 cross[0]=base[1]*norm[2]-base[2]*norm[1];
337 cross[1]=base[2]*norm[0]-base[0]*norm[2];
338 cross[2]=base[0]*norm[1]-base[1]*norm[0];
339 return -0.5*(cross[0]*(pt3[0]-pt1[0])+cross[1]*(pt3[1]-pt1[1])+cross[2]*(pt3[2]-pt1[2])); }
340
341
Geo_QuadArea(double * pt1,double * pt2,double * pt3,double * pt4,int dim)342 double Geo_QuadArea(double *pt1,double *pt2,double *pt3,double *pt4,int dim) {
343 double area,norm[3];
344
345 if(dim==2) {
346 area=Geo_TriArea2(pt1,pt2,pt3);
347 area+=Geo_TriArea2(pt1,pt3,pt4); }
348 else if(dim==3) {
349 area=Geo_TriNormal(pt1,pt2,pt3,norm);
350 if(area<EPSILON) area=Geo_TriNormal(pt1,pt3,pt3,norm);
351 else area+=Geo_TriArea3(pt1,pt3,pt4,norm); }
352 else area=0;
353 return area; }
354
355
356 /********* Inside point ... ***********/
357
Geo_InsidePoints2(double * pt1,double * pt2,double margin,double * ans1,double * ans2,int dim)358 double Geo_InsidePoints2(double *pt1,double *pt2,double margin,double *ans1,double *ans2,int dim) {
359 int d;
360 double len,delta[3];
361
362 len=0;
363 for(d=0;d<dim;d++) {
364 delta[d]=pt2[d]-pt1[d];
365 len+=delta[d]*delta[d]; }
366 len=sqrt(len);
367 for(d=0;d<dim;d++) {
368 delta[d]=delta[d]/len*margin;
369 ans1[d]=pt1[d]+delta[d];
370 ans2[d]=pt2[d]-delta[d]; }
371 return len; }
372
373
Geo_InsidePoints3(double * pt1,double * pt2,double * pt3,double margin,double * ans1,double * ans2,double * ans3)374 void Geo_InsidePoints3(double *pt1,double *pt2,double *pt3,double margin,double *ans1,double *ans2,double *ans3) {
375 double l122,l232,l312,l12,l23,l31,s,factor;
376
377 l122=(pt2[0]-pt1[0])*(pt2[0]-pt1[0])+(pt2[1]-pt1[1])*(pt2[1]-pt1[1])+(pt2[2]-pt1[2])*(pt2[2]-pt1[2]);
378 l232=(pt3[0]-pt2[0])*(pt3[0]-pt2[0])+(pt3[1]-pt2[1])*(pt3[1]-pt2[1])+(pt3[2]-pt2[2])*(pt3[2]-pt2[2]);
379 l312=(pt1[0]-pt3[0])*(pt1[0]-pt3[0])+(pt1[1]-pt3[1])*(pt1[1]-pt3[1])+(pt1[2]-pt3[2])*(pt1[2]-pt3[2]);
380 l12=sqrt(l122);
381 l23=sqrt(l232);
382 l31=sqrt(l312);
383 s=0.5*(l12+l23+l31);
384
385 factor=margin*sqrt(l12*l31/(s*(s-l23)*(2*l122+2*l312-l232)));
386 ans1[0]=pt1[0]+factor*((pt2[0]-pt1[0])/l12-(pt1[0]-pt3[0])/l31);
387 ans1[1]=pt1[1]+factor*((pt2[1]-pt1[1])/l12-(pt1[1]-pt3[1])/l31);
388 ans1[2]=pt1[2]+factor*((pt2[2]-pt1[2])/l12-(pt1[2]-pt3[2])/l31);
389
390 factor=margin*sqrt(l23*l12/(s*(s-l31)*(2*l232+2*l122-l312)));
391 ans2[0]=pt2[0]+factor*((pt3[0]-pt2[0])/l23-(pt2[0]-pt1[0])/l12);
392 ans2[1]=pt2[1]+factor*((pt3[1]-pt2[1])/l23-(pt2[1]-pt1[1])/l12);
393 ans2[2]=pt2[2]+factor*((pt3[2]-pt2[2])/l23-(pt2[2]-pt1[2])/l12);
394
395 factor=margin*sqrt(l31*l23/(s*(s-l12)*(2*l312+2*l232-l122)));
396 ans3[0]=pt3[0]+factor*((pt1[0]-pt3[0])/l31-(pt3[0]-pt2[0])/l23);
397 ans3[1]=pt3[1]+factor*((pt1[1]-pt3[1])/l31-(pt3[1]-pt2[1])/l23);
398 ans3[2]=pt3[2]+factor*((pt1[2]-pt3[2])/l31-(pt3[2]-pt2[2])/l23);
399
400 return; }
401
402
Geo_InsidePoints32(double ** point,double margin,double ** ans)403 void Geo_InsidePoints32(double **point,double margin,double **ans) {
404 double *pt1,*pt2,*pt3,*en12,*en23,*en31,len1,len2,len3;
405
406 pt1=point[0];
407 pt2=point[1];
408 pt3=point[2];
409 en12=point[3];
410 en23=point[4];
411 en31=point[5];
412
413 len1=margin/(1.0+en31[0]*en12[0]+en31[1]*en12[1]+en31[2]+en12[2]);
414 len2=margin/(1.0+en12[0]*en23[0]+en12[1]*en23[1]+en12[2]+en23[2]);
415 len3=margin/(1.0+en23[0]*en31[0]+en23[1]*en31[1]+en23[2]+en31[2]);
416
417 ans[0][0]=pt1[0]-len1*en31[0]-len1*en12[0];
418 ans[0][1]=pt1[1]-len1*en31[1]-len1*en12[1];
419 ans[0][2]=pt1[2]-len1*en31[2]-len1*en12[2];
420 ans[1][0]=pt2[0]-len2*en12[0]-len2*en23[0];
421 ans[1][1]=pt2[1]-len2*en12[1]-len2*en23[1];
422 ans[1][2]=pt2[2]-len2*en12[2]-len2*en23[2];
423 ans[2][0]=pt3[0]-len3*en23[0]-len3*en31[0];
424 ans[2][1]=pt3[1]-len3*en23[1]-len3*en31[1];
425 ans[2][2]=pt3[2]-len3*en23[2]-len3*en31[2];
426
427 return; }
428
429
430
431 /********* Point In ... ***********/
432
Geo_PtInTriangle(double * pt1,double * pt2,double * pt3,double * norm,double * test)433 int Geo_PtInTriangle(double *pt1,double *pt2,double *pt3,double *norm,double *test) {
434 double dx1,dy1,dz1,dx2,dy2,dz2,crss[3],dot;
435
436 dx1=pt2[0]-pt1[0];
437 dy1=pt2[1]-pt1[1];
438 dz1=pt2[2]-pt1[2];
439 dx2=test[0]-pt2[0];
440 dy2=test[1]-pt2[1];
441 dz2=test[2]-pt2[2];
442 crss[0]=dy1*dz2-dz1*dy2;
443 crss[1]=-dx1*dz2+dz1*dx2;
444 crss[2]=dx1*dy2-dy1*dx2;
445 dot=crss[0]*norm[0]+crss[1]*norm[1]+crss[2]*norm[2];
446 if(dot<0) return 0;
447
448 dx1=pt3[0]-pt2[0];
449 dy1=pt3[1]-pt2[1];
450 dz1=pt3[2]-pt2[2];
451 dx2=test[0]-pt3[0];
452 dy2=test[1]-pt3[1];
453 dz2=test[2]-pt3[2];
454 crss[0]=dy1*dz2-dz1*dy2;
455 crss[1]=-dx1*dz2+dz1*dx2;
456 crss[2]=dx1*dy2-dy1*dx2;
457 dot=crss[0]*norm[0]+crss[1]*norm[1]+crss[2]*norm[2];
458 if(dot<0) return 0;
459
460 dx1=pt1[0]-pt3[0];
461 dy1=pt1[1]-pt3[1];
462 dz1=pt1[2]-pt3[2];
463 dx2=test[0]-pt1[0];
464 dy2=test[1]-pt1[1];
465 dz2=test[2]-pt1[2];
466 crss[0]=dy1*dz2-dz1*dy2;
467 crss[1]=-dx1*dz2+dz1*dx2;
468 crss[2]=dx1*dy2-dy1*dx2;
469 dot=crss[0]*norm[0]+crss[1]*norm[1]+crss[2]*norm[2];
470 if(dot<0) return 0;
471 return 1; }
472
473
Geo_PtInTriangle2(double ** point,double * test)474 int Geo_PtInTriangle2(double **point,double *test) {
475 double *pt,*en,dot;
476
477 pt=point[0];
478 en=point[3];
479 dot=(test[0]-pt[0])*en[0]+(test[1]-pt[1])*en[1]+(test[2]-pt[2])*en[2];
480 if(dot>0) return 0;
481
482 pt=point[1];
483 en=point[4];
484 dot=(test[0]-pt[0])*en[0]+(test[1]-pt[1])*en[1]+(test[2]-pt[2])*en[2];
485 if(dot>0) return 0;
486
487 pt=point[2];
488 en=point[5];
489 dot=(test[0]-pt[0])*en[0]+(test[1]-pt[1])*en[1]+(test[2]-pt[2])*en[2];
490 if(dot>0) return 0;
491
492 return 1; }
493
494
Geo_PtInSlab(double * pt1,double * pt2,double * test,int dim)495 int Geo_PtInSlab(double *pt1,double *pt2,double *test,int dim) {
496 double dot;
497 int d;
498
499 dot=0;
500 for(d=0;d<dim;d++) dot+=(test[d]-pt1[d])*(pt2[d]-pt1[d]);
501 if(dot<0) return 0;
502 dot=0;
503 for(d=0;d<dim;d++) dot+=(test[d]-pt2[d])*(pt1[d]-pt2[d]);
504 if(dot<0) return 0;
505 return 1; }
506
507
Geo_PtInSphere(double * test,double * cent,double rad,int dim)508 int Geo_PtInSphere(double *test,double *cent,double rad,int dim) {
509 int d;
510 double r2;
511
512 r2=0;
513 for(d=0;d<dim;d++) r2+=(test[d]-cent[d])*(test[d]-cent[d]);
514 return r2>rad*rad?0:1; }
515
516
517 /*************** Nearest ****************/
518
Geo_NearestSlabPt(double * pt1,double * pt2,double * point,double * ans,int dim)519 void Geo_NearestSlabPt(double *pt1,double *pt2,double *point,double *ans,int dim) {
520 double x,thick;
521 int d;
522
523 x=thick=0;
524 for(d=0;d<dim;d++) {
525 x+=(point[d]-pt1[d])*(pt2[d]-pt1[d]);
526 thick+=(pt2[d]-pt1[d])*(pt2[d]-pt1[d]); }
527 x/=thick;
528 if(x<0)
529 for(d=0;d<dim;d++) ans[d]=point[d]-x*(pt2[d]-pt1[d]);
530 else if(x>1)
531 for(d=0;d<dim;d++) ans[d]=point[d]+(1.0-x)*(pt2[d]-pt1[d]);
532 else
533 for(d=0;d<dim;d++) ans[d]=point[d];
534 return; }
535
536
Geo_NearestLineSegPt(double * pt1,double * pt2,double * point,double * ans,int dim,double margin)537 int Geo_NearestLineSegPt(double *pt1,double *pt2,double *point,double *ans,int dim,double margin) {
538 double x,thick;
539 int d,edgenum;
540
541 x=thick=0;
542 for(d=0;d<dim;d++) {
543 x+=(point[d]-pt1[d])*(pt2[d]-pt1[d]);
544 thick+=(pt2[d]-pt1[d])*(pt2[d]-pt1[d]); }
545 x/=thick;
546 margin/=sqrt(thick);
547 if(x<=margin) {
548 for(d=0;d<dim;d++) ans[d]=pt1[d];
549 edgenum=1; }
550 else if(x>=1-margin) {
551 for(d=0;d<dim;d++) ans[d]=pt2[d];
552 edgenum=2; }
553 else {
554 for(d=0;d<dim;d++) ans[d]=pt1[d]+x*(pt2[d]-pt1[d]);
555 edgenum=0; }
556 return edgenum; }
557
558
Geo_NearestTriPt(double * pt1,double * pt2,double * pt3,double * norm,double * point,double * ans)559 void Geo_NearestTriPt(double *pt1,double *pt2,double *pt3,double *norm,double *point,double *ans) {
560 double dx1,dy1,dz1,dx2,dy2,dz2,crss[3],dot,cross12,cross23,cross31,len2;
561 int d,corner;
562
563 dx1=pt2[0]-pt1[0];
564 dy1=pt2[1]-pt1[1];
565 dz1=pt2[2]-pt1[2];
566 dx2=point[0]-pt2[0];
567 dy2=point[1]-pt2[1];
568 dz2=point[2]-pt2[2];
569 crss[0]=dy1*dz2-dz1*dy2;
570 crss[1]=-dx1*dz2+dz1*dx2;
571 crss[2]=dx1*dy2-dy1*dx2;
572 cross12=crss[0]*norm[0]+crss[1]*norm[1]+crss[2]*norm[2];
573
574 dx1=pt3[0]-pt2[0];
575 dy1=pt3[1]-pt2[1];
576 dz1=pt3[2]-pt2[2];
577 dx2=point[0]-pt3[0];
578 dy2=point[1]-pt3[1];
579 dz2=point[2]-pt3[2];
580 crss[0]=dy1*dz2-dz1*dy2;
581 crss[1]=-dx1*dz2+dz1*dx2;
582 crss[2]=dx1*dy2-dy1*dx2;
583 cross23=crss[0]*norm[0]+crss[1]*norm[1]+crss[2]*norm[2];
584
585 dx1=pt1[0]-pt3[0];
586 dy1=pt1[1]-pt3[1];
587 dz1=pt1[2]-pt3[2];
588 dx2=point[0]-pt1[0];
589 dy2=point[1]-pt1[1];
590 dz2=point[2]-pt1[2];
591 crss[0]=dy1*dz2-dz1*dy2;
592 crss[1]=-dx1*dz2+dz1*dx2;
593 crss[2]=dx1*dy2-dy1*dx2;
594 cross31=crss[0]*norm[0]+crss[1]*norm[1]+crss[2]*norm[2];
595
596 corner=0;
597 if(cross12>=0 && cross23>=0 && cross31>=0)
598 for(d=0;d<3;d++) ans[d]=point[d];
599 else if(cross12<0) {
600 dx2=pt2[0]-pt1[0];
601 dy2=pt2[1]-pt1[1];
602 dz2=pt2[2]-pt1[2];
603 len2=dx2*dx2+dy2*dy2+dz2*dz2;
604 dot=dx2*(point[0]-pt1[0])+dy2*(point[1]-pt1[1])+dz2*(point[2]-pt1[2]);
605 dot/=len2;
606 if(dot<=0) corner=1;
607 else if(dot>=1) corner=2;
608 else {
609 dx1=norm[0];
610 dy1=norm[1];
611 dz1=norm[2];
612 crss[0]=dy1*dz2-dz1*dy2;
613 crss[1]=-dx1*dz2+dz1*dx2;
614 crss[2]=dx1*dy2-dy1*dx2;
615 cross12/=len2;
616 for(d=0;d<3;d++) ans[d]=point[d]-cross12*crss[d]; }}
617 else if(cross23<0) {
618 dx2=pt3[0]-pt2[0];
619 dy2=pt3[1]-pt2[1];
620 dz2=pt3[2]-pt2[2];
621 len2=dx2*dx2+dy2*dy2+dz2*dz2;
622 dot=dx2*(point[0]-pt2[0])+dy2*(point[1]-pt2[1])+dz2*(point[2]-pt2[2]);
623 dot/=len2;
624 if(dot<=0) corner=2;
625 else if(dot>=1) corner=3;
626 else {
627 dx1=norm[0];
628 dy1=norm[1];
629 dz1=norm[2];
630 crss[0]=dy1*dz2-dz1*dy2;
631 crss[1]=-dx1*dz2+dz1*dx2;
632 crss[2]=dx1*dy2-dy1*dx2;
633 cross23/=dx2*dx2+dy2*dy2+dz2*dz2;
634 for(d=0;d<3;d++) ans[d]=point[d]-cross23*crss[d]; }}
635 else if(cross31<0) {
636 dx2=pt1[0]-pt3[0];
637 dy2=pt1[1]-pt3[1];
638 dz2=pt1[2]-pt3[2];
639 len2=dx2*dx2+dy2*dy2+dz2*dz2;
640 dot=dx2*(point[0]-pt3[0])+dy2*(point[1]-pt3[1])+dz2*(point[2]-pt3[2]);
641 dot/=len2;
642 if(dot<=0) corner=3;
643 else if(dot>=1) corner=1;
644 else {
645 dx1=norm[0];
646 dy1=norm[1];
647 dz1=norm[2];
648 crss[0]=dy1*dz2-dz1*dy2;
649 crss[1]=-dx1*dz2+dz1*dx2;
650 crss[2]=dx1*dy2-dy1*dx2;
651 cross31/=dx2*dx2+dy2*dy2+dz2*dz2;
652 for(d=0;d<3;d++) ans[d]=point[d]-cross31*crss[d]; }}
653
654 if(!corner);
655 else if(corner==1) {
656 dot=(point[0]-pt1[0])*norm[0]+(point[1]-pt1[1])*norm[1]+(point[2]-pt1[2])*norm[2];
657 for(d=0;d<3;d++) ans[d]=pt1[d]+dot*norm[d]; }
658 else if(corner==2) {
659 dot=(point[0]-pt2[0])*norm[0]+(point[1]-pt2[1])*norm[1]+(point[2]-pt2[2])*norm[2];
660 for(d=0;d<3;d++) ans[d]=pt2[d]+dot*norm[d]; }
661 else if(corner==3) {
662 dot=(point[0]-pt3[0])*norm[0]+(point[1]-pt3[1])*norm[1]+(point[2]-pt3[2])*norm[2];
663 for(d=0;d<3;d++) ans[d]=pt3[d]+dot*norm[d]; }
664
665 return; }
666
667
Geo_NearestTriPt2(double ** point,double ** edgenorm,double * norm,double * testpt,double * ans)668 void Geo_NearestTriPt2(double **point,double **edgenorm,double *norm,double *testpt,double *ans) {
669 double *pt1,*pt2,*pt3,*en12,*en23,*en31,dot12,dot23,dot31,dx2,dy2,dz2,len2,dot;
670 int d,corner;
671
672 pt1=point[0];
673 pt2=point[1];
674 pt3=point[2];
675 en12=edgenorm[0];
676 en23=edgenorm[1];
677 en31=edgenorm[2];
678
679 dot12=(testpt[0]-pt1[0])*en12[0]+(testpt[1]-pt1[1])*en12[1]+(testpt[2]-pt1[2])*en12[2];
680 dot23=(testpt[0]-pt2[0])*en23[0]+(testpt[1]-pt2[1])*en23[1]+(testpt[2]-pt2[2])*en23[2];
681 dot31=(testpt[0]-pt3[0])*en31[0]+(testpt[1]-pt3[1])*en31[1]+(testpt[2]-pt3[2])*en31[2];
682
683 corner=0;
684 if(dot12<=0 && dot23<=0 && dot31<=0)
685 for(d=0;d<3;d++) ans[d]=testpt[d];
686
687 else if(dot12>0) {
688 dx2=pt2[0]-pt1[0];
689 dy2=pt2[1]-pt1[1];
690 dz2=pt2[2]-pt1[2];
691 len2=dx2*dx2+dy2*dy2+dz2*dz2;
692 dot=dx2*(testpt[0]-pt1[0])+dy2*(testpt[1]-pt1[1])+dz2*(testpt[2]-pt1[2]);
693 if(dot<=0) corner=1;
694 else if(dot>=len2) corner=2;
695 else
696 for(d=0;d<3;d++) ans[d]=testpt[d]-dot12*en12[d]; }
697
698 else if(dot23>0) {
699 dx2=pt3[0]-pt2[0];
700 dy2=pt3[1]-pt2[1];
701 dz2=pt3[2]-pt2[2];
702 len2=dx2*dx2+dy2*dy2+dz2*dz2;
703 dot=dx2*(testpt[0]-pt2[0])+dy2*(testpt[1]-pt2[1])+dz2*(testpt[2]-pt2[2]);
704 if(dot<=0) corner=2;
705 else if(dot>=len2) corner=3;
706 else
707 for(d=0;d<3;d++) ans[d]=testpt[d]-dot23*en23[d]; }
708
709 else if(dot31>0) {
710 dx2=pt1[0]-pt3[0];
711 dy2=pt1[1]-pt3[1];
712 dz2=pt1[2]-pt3[2];
713 len2=dx2*dx2+dy2*dy2+dz2*dz2;
714 dot=dx2*(testpt[0]-pt3[0])+dy2*(testpt[1]-pt3[1])+dz2*(testpt[2]-pt3[2]);
715 if(dot<=0) corner=3;
716 else if(dot>=len2) corner=1;
717 else
718 for(d=0;d<3;d++) ans[d]=testpt[d]-dot31*en31[d]; }
719
720 if(!corner);
721 else if(corner==1) {
722 dot=(testpt[0]-pt1[0])*norm[0]+(testpt[1]-pt1[1])*norm[1]+(testpt[2]-pt1[2])*norm[2];
723 for(d=0;d<3;d++) ans[d]=pt1[d]+dot*norm[d]; }
724 else if(corner==2) {
725 dot=(testpt[0]-pt2[0])*norm[0]+(testpt[1]-pt2[1])*norm[1]+(testpt[2]-pt2[2])*norm[2];
726 for(d=0;d<3;d++) ans[d]=pt2[d]+dot*norm[d]; }
727 else if(corner==3) {
728 dot=(testpt[0]-pt3[0])*norm[0]+(testpt[1]-pt3[1])*norm[1]+(testpt[2]-pt3[2])*norm[2];
729 for(d=0;d<3;d++) ans[d]=pt3[d]+dot*norm[d]; }
730
731 return; }
732
733
Geo_NearestTrianglePt(double * pt1,double * pt2,double * pt3,double * norm,double * point,double * ans)734 int Geo_NearestTrianglePt(double *pt1,double *pt2,double *pt3,double *norm,double *point,double *ans) {
735 double dx1,dy1,dz1,dx2,dy2,dz2,crss[3],dot,cross12,cross23,cross31,len2;
736 int d,corner,edgenum;
737
738 edgenum=0;
739
740 dx1=pt2[0]-pt1[0];
741 dy1=pt2[1]-pt1[1];
742 dz1=pt2[2]-pt1[2];
743 dx2=point[0]-pt2[0];
744 dy2=point[1]-pt2[1];
745 dz2=point[2]-pt2[2];
746 crss[0]=dy1*dz2-dz1*dy2;
747 crss[1]=-dx1*dz2+dz1*dx2;
748 crss[2]=dx1*dy2-dy1*dx2;
749 cross12=crss[0]*norm[0]+crss[1]*norm[1]+crss[2]*norm[2];
750
751 dx1=pt3[0]-pt2[0];
752 dy1=pt3[1]-pt2[1];
753 dz1=pt3[2]-pt2[2];
754 dx2=point[0]-pt3[0];
755 dy2=point[1]-pt3[1];
756 dz2=point[2]-pt3[2];
757 crss[0]=dy1*dz2-dz1*dy2;
758 crss[1]=-dx1*dz2+dz1*dx2;
759 crss[2]=dx1*dy2-dy1*dx2;
760 cross23=crss[0]*norm[0]+crss[1]*norm[1]+crss[2]*norm[2];
761
762 dx1=pt1[0]-pt3[0];
763 dy1=pt1[1]-pt3[1];
764 dz1=pt1[2]-pt3[2];
765 dx2=point[0]-pt1[0];
766 dy2=point[1]-pt1[1];
767 dz2=point[2]-pt1[2];
768 crss[0]=dy1*dz2-dz1*dy2;
769 crss[1]=-dx1*dz2+dz1*dx2;
770 crss[2]=dx1*dy2-dy1*dx2;
771 cross31=crss[0]*norm[0]+crss[1]*norm[1]+crss[2]*norm[2];
772
773 corner=0;
774 if(cross12>=0 && cross23>=0 && cross31>=0) {
775 dot=(point[0]-pt1[0])*norm[0]+(point[1]-pt1[1])*norm[1]+(point[2]-pt1[2])*norm[2];
776 for(d=0;d<3;d++) ans[d]=point[d]-dot*norm[d];
777 if(cross12==0) edgenum=1;
778 else if(cross23==0) edgenum=2;
779 else if(cross31==0) edgenum=3;
780 else edgenum=0; }
781 else if(cross12<0) {
782 dx2=pt2[0]-pt1[0];
783 dy2=pt2[1]-pt1[1];
784 dz2=pt2[2]-pt1[2];
785 len2=dx2*dx2+dy2*dy2+dz2*dz2;
786 dot=(point[0]-pt1[0])*dx2+(point[1]-pt1[1])*dy2+(point[2]-pt1[2])*dz2;
787 dot/=len2;
788 if(dot<=0) corner=1;
789 else if(dot>=1) corner=2;
790 else {
791 ans[0]=pt1[0]+dot*dx2;
792 ans[1]=pt1[1]+dot*dy2;
793 ans[2]=pt1[2]+dot*dz2;
794 edgenum=1; }}
795 else if(cross23<0) {
796 dx2=pt3[0]-pt2[0];
797 dy2=pt3[1]-pt2[1];
798 dz2=pt3[2]-pt2[2];
799 len2=dx2*dx2+dy2*dy2+dz2*dz2;
800 dot=(point[0]-pt2[0])*dx2+(point[1]-pt2[1])*dy2+(point[2]-pt2[2])*dz2;
801 dot/=len2;
802 if(dot<=0) corner=2;
803 else if(dot>=1) corner=3;
804 else {
805 ans[0]=pt2[0]+dot*dx2;
806 ans[1]=pt2[1]+dot*dy2;
807 ans[2]=pt2[2]+dot*dz2;
808 edgenum=2; }}
809 else if(cross31<0) {
810 dx2=pt1[0]-pt3[0];
811 dy2=pt1[1]-pt3[1];
812 dz2=pt1[2]-pt3[2];
813 len2=dx2*dx2+dy2*dy2+dz2*dz2;
814 dot=(point[0]-pt3[0])*dx2+(point[1]-pt3[1])*dy2+(point[2]-pt3[2])*dz2;
815 dot/=len2;
816 if(dot<=0) corner=3;
817 else if(dot>=1) corner=1;
818 else {
819 ans[0]=pt3[0]+dot*dx2;
820 ans[1]=pt3[1]+dot*dy2;
821 ans[2]=pt3[2]+dot*dz2;
822 edgenum=3; }}
823
824 if(!corner);
825 else if(corner==1) {
826 for(d=0;d<3;d++) ans[d]=pt1[d];
827 edgenum=1; }
828 else if(corner==2) {
829 for(d=0;d<3;d++) ans[d]=pt2[d];
830 edgenum=2; }
831 else if(corner==3) {
832 for(d=0;d<3;d++) ans[d]=pt3[d];
833 edgenum=3; }
834
835 return edgenum; }
836
837
Geo_NearestTrianglePt2(double ** point,double * norm,double * testpt,double * ans,double margin)838 int Geo_NearestTrianglePt2(double **point,double *norm,double *testpt,double *ans,double margin) {
839 double *pt1,*pt2,*pt3,*en12,*en23,*en31,dot12,dot23,dot31,dx2,dy2,dz2,len2,dot;
840 int d,corner,edgenum;
841
842 edgenum=0;
843
844 pt1=point[0];
845 pt2=point[1];
846 pt3=point[2];
847 en12=point[3];
848 en23=point[4];
849 en31=point[5];
850
851 dot12=(testpt[0]-pt1[0])*en12[0]+(testpt[1]-pt1[1])*en12[1]+(testpt[2]-pt1[2])*en12[2];
852 dot23=(testpt[0]-pt2[0])*en23[0]+(testpt[1]-pt2[1])*en23[1]+(testpt[2]-pt2[2])*en23[2];
853 dot31=(testpt[0]-pt3[0])*en31[0]+(testpt[1]-pt3[1])*en31[1]+(testpt[2]-pt3[2])*en31[2];
854
855 corner=0;
856 if(dot12<=0 && dot23<=0 && dot31<=0) {
857 dot=(testpt[0]-pt1[0])*norm[0]+(testpt[1]-pt1[1])*norm[1]+(testpt[2]-pt1[2])*norm[2];
858 for(d=0;d<3;d++) ans[d]=testpt[d]-dot*norm[d];
859 if(dot12>-margin) edgenum=1;
860 else if(dot23>-margin) edgenum=2;
861 else if(dot31>-margin) edgenum=3;
862 else edgenum=0; }
863
864 else if(dot12>0) {
865 dx2=pt2[0]-pt1[0];
866 dy2=pt2[1]-pt1[1];
867 dz2=pt2[2]-pt1[2];
868 len2=dx2*dx2+dy2*dy2+dz2*dz2;
869 dot=dx2*(testpt[0]-pt1[0])+dy2*(testpt[1]-pt1[1])+dz2*(testpt[2]-pt1[2]);
870 if(dot<=0) corner=1;
871 else if(dot>=len2) corner=2;
872 else {
873 dot/=len2;
874 ans[0]=pt1[0]+dot*dx2;
875 ans[1]=pt1[1]+dot*dy2;
876 ans[2]=pt1[2]+dot*dz2;
877 edgenum=1; }}
878
879 else if(dot23>0) {
880 dx2=pt3[0]-pt2[0];
881 dy2=pt3[1]-pt2[1];
882 dz2=pt3[2]-pt2[2];
883 len2=dx2*dx2+dy2*dy2+dz2*dz2;
884 dot=dx2*(testpt[0]-pt2[0])+dy2*(testpt[1]-pt2[1])+dz2*(testpt[2]-pt2[2]);
885 if(dot<=0) corner=2;
886 else if(dot>=len2) corner=3;
887 else {
888 dot/=len2;
889 ans[0]=pt2[0]+dot*dx2;
890 ans[1]=pt2[1]+dot*dy2;
891 ans[2]=pt2[2]+dot*dz2;
892 edgenum=2; }}
893
894 else if(dot31>0) {
895 dx2=pt1[0]-pt3[0];
896 dy2=pt1[1]-pt3[1];
897 dz2=pt1[2]-pt3[2];
898 len2=dx2*dx2+dy2*dy2+dz2*dz2;
899 dot=dx2*(testpt[0]-pt3[0])+dy2*(testpt[1]-pt3[1])+dz2*(testpt[2]-pt3[2]);
900 if(dot<=0) corner=3;
901 else if(dot>=len2) corner=1;
902 else {
903 dot/=len2;
904 ans[0]=pt3[0]+dot*dx2;
905 ans[1]=pt3[1]+dot*dy2;
906 ans[2]=pt3[2]+dot*dz2;
907 edgenum=3; }}
908
909 if(!corner);
910 else if(corner==1) {
911 for(d=0;d<3;d++) ans[d]=pt1[d];
912 edgenum=1; }
913 else if(corner==2) {
914 for(d=0;d<3;d++) ans[d]=pt2[d];
915 edgenum=2; }
916 else if(corner==3) {
917 for(d=0;d<3;d++) ans[d]=pt3[d];
918 edgenum=3; }
919
920 return edgenum; }
921
922
Geo_NearestSpherePt(double * cent,double rad,int front,int dim,double * point,double * ans)923 double Geo_NearestSpherePt(double *cent,double rad,int front,int dim,double *point,double *ans) {
924 double dist,vect[3];
925 int d;
926
927 dist=Geo_SphereNormal(cent,point,front,dim,vect);
928 for(d=0;d<dim;d++) ans[d]=cent[d]+rad*vect[d];
929 return front*(dist-rad); }
930
931
Geo_NearestRingPt(double * cent,double * axis,double rad,int dim,double * point,double * ans)932 void Geo_NearestRingPt(double *cent,double *axis,double rad,int dim,double *point,double *ans) {
933 int d;
934 double dot,vect[3];
935
936 dot=0;
937 for(d=0;d<dim;d++) {
938 vect[d]=point[d]-cent[d];
939 dot+=vect[d]*axis[d]; }
940 for(d=0;d<dim;d++) vect[d]-=dot*axis[d];
941 dot=0;
942 for(d=0;d<dim;d++) dot+=vect[d]*vect[d];
943 dot=rad/sqrt(dot);
944 for(d=0;d<dim;d++) ans[d]=cent[d]+dot*vect[d];
945 return; }
946
947
Geo_NearestCylPt(double * pt1,double * axis,double rad,int dim,double * point,double * ans)948 void Geo_NearestCylPt(double *pt1,double *axis,double rad,int dim,double *point,double *ans) {
949 int d;
950 double dot,vect[3];
951
952 dot=0;
953 for(d=0;d<dim;d++) {
954 vect[d]=point[d]-pt1[d]; // vect points from pt1 to point
955 dot+=vect[d]*axis[d]; } // dot is dot product of vect with axis
956 for(d=0;d<dim;d++) vect[d]-=dot*axis[d]; // vect is now from cylinder axis to point
957 dot=0;
958 for(d=0;d<dim;d++) dot+=vect[d]*vect[d]; // dot is now squared length of new vect
959 if(dot<=rad*rad)
960 for(d=0;d<dim;d++) ans[d]=point[d]; // if point is inside cylinder, then answer is that point
961 else {
962 dot=sqrt(dot); // dot is now length of new vect
963 dot=1.0-rad/dot; // dot is now subtraction factor
964 for(d=0;d<dim;d++) ans[d]=point[d]-dot*vect[d]; } // find point on cylinder surface closest to point
965 return; }
966
967
Geo_NearestCylinderPt(double * pt1,double * pt2,double rad,int dim,double * point,double * ans,double margin)968 int Geo_NearestCylinderPt(double *pt1,double *pt2,double rad,int dim,double *point,double *ans,double margin) {
969 int d,edgenum;
970 double dot,len,len2,vect[3];
971
972 dot=len=0;
973 for(d=0;d<dim;d++) {
974 vect[d]=point[d]-pt1[d]; // vect points from pt1 to point
975 dot+=vect[d]*(pt2[d]-pt1[d]); // dot is dot product of vect with cylinder axis
976 len+=(pt2[d]-pt1[d])*(pt2[d]-pt1[d]); } // len is squared cylinder length
977 dot/=len; // dot is now length of vect along cyl. axis, divided by the axis length
978 margin/=sqrt(len);
979 for(d=0;d<dim;d++) vect[d]-=dot*(pt2[d]-pt1[d]); // vect now points from cyl. axis to point
980 if(dot<=margin) {dot=0;edgenum=1;}
981 else if(dot>=1-margin) {dot=1;edgenum=2;}
982 else edgenum=0;
983 len2=0;
984 for(d=0;d<dim;d++) len2+=vect[d]*vect[d]; // len2 is squared length of distance from cyl. axis to point
985 len2=rad/sqrt(len2); // len2 is ratio of radius to length from cyl. axis to point
986 for(d=0;d<dim;d++) ans[d]=pt1[d]+dot*(pt2[d]-pt1[d])+len2*vect[d];
987 return edgenum; }
988
989
Geo_NearestDiskPt(double * cent,double * axis,double rad,int dim,double * point,double * ans,double margin)990 int Geo_NearestDiskPt(double *cent,double *axis,double rad,int dim,double *point,double *ans,double margin) {
991 int d,edgenum;
992 double dot,vect[3];
993
994 dot=0;
995 for(d=0;d<dim;d++) {
996 vect[d]=point[d]-cent[d]; // vect points from center to point
997 dot+=vect[d]*axis[d]; } // dot is dot product of vect with axis
998 for(d=0;d<dim;d++) vect[d]-=dot*axis[d]; // vect now points from center to point projected to infinite disk
999 dot=0;
1000 for(d=0;d<dim;d++) dot+=vect[d]*vect[d]; // dot is now squared length of vect
1001 dot=sqrt(dot); // dot is now length of vect
1002 if(dot>=rad-margin) {
1003 dot=rad/dot;
1004 edgenum=1; }
1005 else {
1006 dot=1;
1007 edgenum=0; }
1008 for(d=0;d<dim;d++) ans[d]=cent[d]+dot*vect[d];
1009 return edgenum; }
1010
1011
Geo_NearestLine2LineDist(double * ptA1,double * ptA2,double * ptB1,double * ptB2)1012 double Geo_NearestLine2LineDist(double *ptA1,double *ptA2,double *ptB1,double *ptB2) {
1013 double a,b,c,d,e,denom,sc,tc,dist,vect[3];
1014
1015 a=(ptA2[0]-ptA1[0])*(ptA2[0]-ptA1[0])+(ptA2[1]-ptA1[1])*(ptA2[1]-ptA1[1])+(ptA2[2]-ptA1[2])*(ptA2[2]-ptA1[2]);
1016 b=(ptA2[0]-ptA1[0])*(ptB2[0]-ptB1[0])+(ptA2[1]-ptA1[1])*(ptB2[1]-ptB1[1])+(ptA2[2]-ptA1[2])*(ptB2[2]-ptB1[2]);
1017 c=(ptB2[0]-ptB1[0])*(ptB2[0]-ptB1[0])+(ptB2[1]-ptB1[1])*(ptB2[1]-ptB1[1])+(ptB2[2]-ptB1[2])*(ptB2[2]-ptB1[2]);
1018 d=(ptA2[0]-ptA1[0])*(ptA1[0]-ptB1[0])+(ptA2[1]-ptA1[1])*(ptA1[1]-ptB1[1])+(ptA2[2]-ptA1[2])*(ptA1[2]-ptB1[2]);
1019 e=(ptB2[0]-ptB1[0])*(ptA1[0]-ptB1[0])+(ptB2[1]-ptB1[1])*(ptA1[1]-ptB1[1])+(ptB2[2]-ptB1[2])*(ptA1[2]-ptB1[2]);
1020 denom=a*c-b*b;
1021 if(denom<EPSILON) {
1022 sc=0.0;
1023 tc=b>c?d/b:e/c; }
1024 else {
1025 sc=(b*e-c*d)/denom;
1026 tc=(a*e-b*d)/denom; }
1027 vect[0]=ptA1[0]+sc*(ptA2[0]-ptA1[0])-ptB1[0]-tc*(ptB2[0]-ptB1[0]);
1028 vect[1]=ptA1[1]+sc*(ptA2[1]-ptA1[1])-ptB1[1]-tc*(ptB2[1]-ptB1[1]);
1029 vect[2]=ptA1[2]+sc*(ptA2[2]-ptA1[2])-ptB1[2]-tc*(ptB2[2]-ptB1[2]);
1030 dist=sqrt(vect[0]*vect[0]+vect[1]*vect[1]+vect[2]*vect[2]);
1031 return dist; }
1032
1033
Geo_NearestSeg2SegDist(double * ptA1,double * ptA2,double * ptB1,double * ptB2)1034 double Geo_NearestSeg2SegDist(double *ptA1,double *ptA2,double *ptB1,double *ptB2) {
1035 double a,b,c,d,e,denom,sc,tc,dist,vect[3];
1036 double sn,tn,sd,td;
1037
1038 a=(ptA2[0]-ptA1[0])*(ptA2[0]-ptA1[0])+(ptA2[1]-ptA1[1])*(ptA2[1]-ptA1[1])+(ptA2[2]-ptA1[2])*(ptA2[2]-ptA1[2]);
1039 b=(ptA2[0]-ptA1[0])*(ptB2[0]-ptB1[0])+(ptA2[1]-ptA1[1])*(ptB2[1]-ptB1[1])+(ptA2[2]-ptA1[2])*(ptB2[2]-ptB1[2]);
1040 c=(ptB2[0]-ptB1[0])*(ptB2[0]-ptB1[0])+(ptB2[1]-ptB1[1])*(ptB2[1]-ptB1[1])+(ptB2[2]-ptB1[2])*(ptB2[2]-ptB1[2]);
1041 d=(ptA2[0]-ptA1[0])*(ptA1[0]-ptB1[0])+(ptA2[1]-ptA1[1])*(ptA1[1]-ptB1[1])+(ptA2[2]-ptA1[2])*(ptA1[2]-ptB1[2]);
1042 e=(ptB2[0]-ptB1[0])*(ptA1[0]-ptB1[0])+(ptB2[1]-ptB1[1])*(ptA1[1]-ptB1[1])+(ptB2[2]-ptB1[2])*(ptA1[2]-ptB1[2]);
1043 denom=a*c-b*b;
1044 sd=denom;
1045 td=denom;
1046 if(denom<EPSILON) {
1047 sn=0.0;
1048 sd=1.0;
1049 tn=e;
1050 td=c; }
1051 else {
1052 sn=b*e-c*d;
1053 tn=a*e-b*d;
1054 if(sn<0.0) {
1055 sn=0.0;
1056 tn=e;
1057 td=c; }
1058 else if(sn>sd) {
1059 sn=sd;
1060 tn=e+b;
1061 td=c; }}
1062
1063 if(tn<0.0) {
1064 tn=0.0;
1065 if(-d<0.0) sn=0.0;
1066 else if(-d>a) sn=sd;
1067 else {
1068 sn=-d;
1069 sd=a; }}
1070 else if(tn>td) {
1071 tn=td;
1072 if((-d+b)<0.0) sn=0;
1073 else if((-d+b)>a) sn=sd;
1074 else {
1075 sn=(-d+b);
1076 sd=a; }}
1077
1078 sc=(fabs(sn)<EPSILON?0.0:sn/sd);
1079 tc=(fabs(tn)<EPSILON?0.0:tn/td);
1080 vect[0]=ptA1[0]+sc*(ptA2[0]-ptA1[0])-ptB1[0]-tc*(ptB2[0]-ptB1[0]);
1081 vect[1]=ptA1[1]+sc*(ptA2[1]-ptA1[1])-ptB1[1]-tc*(ptB2[1]-ptB1[1]);
1082 vect[2]=ptA1[2]+sc*(ptA2[2]-ptA1[2])-ptB1[2]-tc*(ptB2[2]-ptB1[2]);
1083 dist=sqrt(vect[0]*vect[0]+vect[1]*vect[1]+vect[2]*vect[2]);
1084
1085 return dist; }
1086
1087
Geo_NearestAabbPt(const double * bpt1,const double * bpt2,int dim,const double * point,double * ans)1088 double Geo_NearestAabbPt(const double *bpt1,const double *bpt2,int dim,const double *point,double *ans) {
1089 int d;
1090 double dist;
1091
1092 dist=0;
1093 for(d=0;d<dim;d++) {
1094 if(point[d]<=bpt1[d]) ans[d]=bpt1[d];
1095 else if(point[d]>=bpt2[d]) ans[d]=bpt2[d];
1096 else ans[d]=point[d];
1097 dist+=(ans[d]-point[d])*(ans[d]-point[d]); }
1098 return sqrt(dist); }
1099
1100
1101 /*************** To Rect ****************/
1102
Geo_Semic2Rect(double * cent,double rad,double * outvect,double * r1,double * r2,double * r3)1103 void Geo_Semic2Rect(double *cent,double rad,double *outvect,double *r1,double *r2,double *r3) {
1104 r1[0]=cent[0]+rad*outvect[1];
1105 r1[1]=cent[1]-rad*outvect[0];
1106 r2[0]=cent[0]-rad*outvect[1];
1107 r2[1]=cent[1]+rad*outvect[0];
1108 r3[0]=cent[0]+rad*outvect[1]-rad*outvect[0];
1109 r3[1]=cent[1]-rad*outvect[0]-rad*outvect[1];
1110 return; }
1111
1112
Geo_Hemis2Rect(double * cent,double rad,double * outvect,double * r1,double * r2,double * r3,double * r4)1113 void Geo_Hemis2Rect(double *cent,double rad,double *outvect,double *r1,double *r2,double *r3,double *r4) {
1114 double v1[3],v2[3];
1115
1116 v2[0]=v2[1]=v2[2]=0;
1117 Geo_LineNormal3D(v2,outvect,v2,v1);
1118 v2[0]=v1[1]*outvect[2]-v1[2]*outvect[1]; // v2 = v1 x outvect
1119 v2[1]=v1[2]*outvect[0]-v1[0]*outvect[2];
1120 v2[2]=v1[0]*outvect[1]-v1[1]*outvect[0];
1121
1122 r1[0]=cent[0]+rad*(-v1[0]-v2[0]);
1123 r1[1]=cent[1]+rad*(-v1[1]-v2[1]);
1124 r1[2]=cent[2]+rad*(-v1[2]-v2[2]);
1125 r2[0]=cent[0]+rad*(+v1[0]-v2[0]);
1126 r2[1]=cent[1]+rad*(+v1[1]-v2[1]);
1127 r2[2]=cent[2]+rad*(+v1[2]-v2[2]);
1128 r3[0]=cent[0]+rad*(-v1[0]+v2[0]);
1129 r3[1]=cent[1]+rad*(-v1[1]+v2[1]);
1130 r3[2]=cent[2]+rad*(-v1[2]+v2[2]);
1131 r4[0]=cent[0]+rad*(-v1[0]-v2[0]-outvect[0]);
1132 r4[1]=cent[1]+rad*(-v1[1]-v2[1]-outvect[1]);
1133 r4[2]=cent[2]+rad*(-v1[2]-v2[2]-outvect[2]);
1134 return; }
1135
1136
Geo_Cyl2Rect(double * pt1,double * pt2,double rad,double * r1,double * r2,double * r3,double * r4)1137 void Geo_Cyl2Rect(double *pt1,double *pt2,double rad,double *r1,double *r2,double *r3,double *r4) {
1138 double v1[3],v2[3],nrm;
1139
1140 Geo_LineNormal3D(pt1,pt2,pt1,v1);
1141 v2[0]=v1[1]*(pt2[2]-pt1[2])-v1[2]*(pt2[1]-pt1[1]); // v2 = v1 x (pt2 - pt1)
1142 v2[1]=v1[2]*(pt2[0]-pt1[0])-v1[0]*(pt2[2]-pt1[2]);
1143 v2[2]=v1[0]*(pt2[1]-pt1[1])-v1[1]*(pt2[0]-pt1[0]);
1144 nrm=sqrt(v2[0]*v2[0]+v2[1]*v2[1]+v2[2]*v2[2]); // normalize v2
1145 v2[0]/=nrm;
1146 v2[1]/=nrm;
1147 v2[2]/=nrm;
1148
1149 r1[0]=pt1[0]+rad*(-v1[0]-v2[0]);
1150 r1[1]=pt1[1]+rad*(-v1[1]-v2[1]);
1151 r1[2]=pt1[2]+rad*(-v1[2]-v2[2]);
1152 r2[0]=pt1[0]+rad*(+v1[0]-v2[0]);
1153 r2[1]=pt1[1]+rad*(+v1[1]-v2[1]);
1154 r2[2]=pt1[2]+rad*(+v1[2]-v2[2]);
1155 r3[0]=pt1[0]+rad*(-v1[0]+v2[0]);
1156 r3[1]=pt1[1]+rad*(-v1[1]+v2[1]);
1157 r3[2]=pt1[2]+rad*(-v1[2]+v2[2]);
1158 r4[0]=pt2[0]+rad*(-v1[0]-v2[0]);
1159 r4[1]=pt2[1]+rad*(-v1[1]-v2[1]);
1160 r4[2]=pt2[2]+rad*(-v1[2]-v2[2]);
1161 return; }
1162
1163
1164 /**************** ...X... ***************/
1165
Geo_LineXLine(double * l1p1,double * l1p2,double * l2p1,double * l2p2,double * crss2ptr)1166 double Geo_LineXLine(double *l1p1,double *l1p2,double *l2p1,double *l2p2,double *crss2ptr) {
1167 double rxcx,rycy,sxrx,syry,dxcx,dycy,cross;
1168
1169 rxcx=l2p1[0]-l1p1[0]; // rx-cx
1170 rycy=l2p1[1]-l1p1[1]; // ry-cy
1171 sxrx=l2p2[0]-l2p1[0]; // sx-rx
1172 syry=l2p2[1]-l2p1[1]; // sy-ry
1173 dxcx=l1p2[0]-l1p1[0]; // dx-cx
1174 dycy=l1p2[1]-l1p1[1]; // dy-cy
1175 cross=(rxcx*syry-rycy*sxrx)/(dxcx*syry-dycy*sxrx);
1176 if(crss2ptr) *crss2ptr=(rxcx*dycy-rycy*dxcx)/(dxcx*syry-dycy*sxrx);
1177 return cross; }
1178
1179
Geo_LineXPlane(double * pt1,double * pt2,double * v,double * norm,double * crsspt)1180 double Geo_LineXPlane(double *pt1,double *pt2,double *v,double *norm,double *crsspt) {
1181 double lx,ly,lz,sx,sy,sz,a;
1182
1183 lx=pt2[0]-pt1[0];
1184 ly=pt2[1]-pt1[1];
1185 lz=pt2[2]-pt1[2];
1186
1187 sx=v[0]-pt1[0];
1188 sy=v[1]-pt1[1];
1189 sz=v[2]-pt1[2];
1190
1191 a=(sx*norm[0]+sy*norm[1]+sz*norm[2])/(lx*norm[0]+ly*norm[1]+lz*norm[2]);
1192
1193 crsspt[0]=pt1[0]+a*lx;
1194 crsspt[1]=pt1[1]+a*ly;
1195 crsspt[2]=pt1[2]+a*lz;
1196
1197 return a; }
1198
1199
Geo_LineXSphs(double * pt1,double * pt2,double * cent,double rad,int dim,double * crss2ptr,double * nrdistptr,double * nrposptr)1200 double Geo_LineXSphs(double *pt1,double *pt2,double *cent,double rad,int dim,double *crss2ptr,double *nrdistptr,double *nrposptr) {
1201 double a,b,c,cross,cross2,radical,nrdist;
1202 int d;
1203
1204 a=b=c=0;
1205 for(d=0;d<dim;d++) {
1206 a+=(pt2[d]-pt1[d])*(pt2[d]-pt1[d]);
1207 b+=(cent[d]-pt1[d])*(pt2[d]-pt1[d]);
1208 c+=(pt1[d]-cent[d])*(pt1[d]-cent[d]); }
1209 nrdist=sqrt(c-b*b/a);
1210 if(nrdistptr) *nrdistptr=nrdist;
1211 if(nrposptr) *nrposptr=b/a;
1212 b*=-2;
1213 c-=rad*rad;
1214 radical=b*b-4*a*c;
1215 if(nrdist<=rad && radical<0) radical=0;
1216 radical=sqrt(radical);
1217 cross=(-b-radical)/(2*a);
1218 cross2=(-b+radical)/(2*a);
1219 if(crss2ptr) *crss2ptr=cross2;
1220 return cross; }
1221
1222
Geo_LineXCyl2s(double * pt1,double * pt2,double * cp1,double * cp2,double * norm,double rad,double * crss2ptr,double * nrdistptr,double * nrposptr)1223 double Geo_LineXCyl2s(double *pt1,double *pt2,double *cp1,double *cp2,double *norm,double rad,double *crss2ptr,double *nrdistptr,double *nrposptr) {
1224 double edge0[2],edge1[2],crossl,crossr,nrpos,nrdist;
1225
1226 edge0[0]=cp1[0]+norm[0]*rad;
1227 edge0[1]=cp1[1]+norm[1]*rad;
1228 edge1[0]=cp2[0]+norm[0]*rad;
1229 edge1[1]=cp2[1]+norm[1]*rad;
1230 crossr=Geo_LineXLine(pt1,pt2,edge0,edge1,NULL);
1231 edge0[0]=cp1[0]-norm[0]*rad;
1232 edge0[1]=cp1[1]-norm[1]*rad;
1233 edge1[0]=cp2[0]-norm[0]*rad;
1234 edge1[1]=cp2[1]-norm[1]*rad;
1235 crossl=Geo_LineXLine(pt1,pt2,edge0,edge1,NULL);
1236 nrpos=Geo_LineXLine(pt1,pt2,cp1,cp2,NULL);
1237 if(!(nrpos>=0 || nrpos<0)) Geo_LineNormPos(cp1,cp2,pt1,2,&nrdist);
1238 else nrdist=0;
1239 if(nrdistptr) *nrdistptr=nrdist;
1240 if(nrposptr) *nrposptr=nrpos;
1241 if(crossr<crossl) {
1242 if(crss2ptr) *crss2ptr=crossl;
1243 return crossr; }
1244 if(crss2ptr) *crss2ptr=crossr;
1245 return crossl; }
1246
1247
Geo_LineXCyls(double * pt1,double * pt2,double * cp1,double * cp2,double rad,double * crss2ptr,double * nrdistptr,double * nrposptr)1248 double Geo_LineXCyls(double *pt1,double *pt2,double *cp1,double *cp2,double rad,double *crss2ptr,double *nrdistptr,double *nrposptr) {
1249 double r1,r2,norm1[3],norm2[3],zero[3];
1250 int d;
1251
1252 r1=Geo_LineNormal3D(cp1,cp2,pt1,norm1);
1253 r2=Geo_LineNormal3D(cp1,cp2,pt2,norm2);
1254 for(d=0;d<3;d++) {
1255 zero[d]=0;
1256 norm1[d]*=r1;
1257 norm2[d]*=r2; }
1258 return Geo_LineXSphs(norm1,norm2,zero,rad,3,crss2ptr,nrdistptr,nrposptr); }
1259
1260
1261 /**************** Reflection functions *************/
1262
1263
Geo_SphereReflectSphere(const double * a0,const double * a1,const double * b0,const double * b1,int dim,double radius2,double * a1p,double * b1p)1264 double Geo_SphereReflectSphere(const double *a0,const double *a1,const double *b0,const double *b1,int dim,double radius2,double *a1p,double *b1p) {
1265 double a,b,c,p,f1,f2,deltap[3];
1266 int d;
1267
1268 a=b=c=0;
1269 for(d=0;d<dim;d++) {
1270 f1=b1[d]-a1[d]-b0[d]+a0[d];
1271 f2=b0[d]-a0[d];
1272 a+=f1*f1;
1273 b+=2*f2*f1;
1274 c+=f2*f2; }
1275 c-=radius2;
1276 p=(-b-sqrt(b*b-4*a*c))/(2*a);
1277
1278 f1=f2=0;
1279 for(d=0;d<dim;d++) {
1280 deltap[d]=(1-p)*(b0[d]-a0[d])+p*(b1[d]-a1[d]);
1281 f1+=(a1[d]-a0[d])*deltap[d];
1282 f2+=(b1[d]-b0[d])*deltap[d]; }
1283 f1*=2*(1-p)/radius2;
1284 f2*=2*(1-p)/radius2;
1285 for(d=0;d<dim;d++) {
1286 a1p[d]=a1[d]-f1*deltap[d];
1287 b1p[d]=b1[d]-f2*deltap[d]; }
1288
1289 return p; }
1290
1291
1292 /******************* Exit functions ****************/
1293
1294
Geo_LineExitRect(double * pt1,double * pt2,double * front,double * corner1,double * corner3,double * exitpt,int * exitside)1295 double Geo_LineExitRect(double *pt1,double *pt2,double *front,double *corner1,double *corner3,double *exitpt,int *exitside) {
1296 int indx1,indx2,xside,yside,side;
1297 double x1,y1,x2,y2,xscale,yscale,a1,a2,a3,a4,ax,ay,a;
1298
1299 indx1=(int)front[2]; // set indx1 to axis parallel to edge from pt 0 to pt 1
1300 indx2=(indx1+1)%3; // set indx2 to axis parallel to edge from pt 1 to pt 2
1301 if(indx2==(int)front[1]) indx2=(indx2+1)%3;
1302
1303 x1=pt1[indx1]; // coordinates of the line ends
1304 y1=pt1[indx2]; // x and y actually are x and y axes if axis is 2 and different otherwise
1305 x2=pt2[indx1];
1306 y2=pt2[indx2];
1307
1308 xscale=(x2!=x1)?1.0/(x2-x1):INFINITY;
1309 yscale=(y2!=y1)?1.0/(y2-y1):INFINITY;
1310
1311 a1=(corner1[indx2]-y1)*yscale;
1312 a2=(corner3[indx1]-x1)*xscale;
1313 a3=(corner3[indx2]-y1)*yscale;
1314 a4=(corner1[indx1]-x1)*xscale;
1315
1316 if(a1>a3) {ay=a1;yside=1;}
1317 else {ay=a3;yside=3;}
1318
1319 if(a2>a4) {ax=a2;xside=2;}
1320 else {ax=a4;xside=4;}
1321
1322 if((ay<ax && y1!=y2) || x1==x2) {a=ay;side=yside;}
1323 else {a=ax;side=xside;}
1324
1325 exitpt[0]=pt1[0]+a*(pt2[0]-pt1[0]);
1326 exitpt[1]=pt1[1]+a*(pt2[1]-pt1[1]);
1327 exitpt[2]=pt1[2]+a*(pt2[2]-pt1[2]);
1328
1329 *exitside=side;
1330
1331 return a; }
1332
1333
Geo_LineExitLine2(double * pt1,double * pt2,double * end1,double * end2,double * exitpt,int * exitend)1334 double Geo_LineExitLine2(double *pt1,double *pt2,double *end1,double *end2,double *exitpt,int *exitend) {
1335 int i1;
1336 double a,a1,a2;
1337
1338 i1=(fabs(pt2[1]-pt1[1])>fabs(pt2[0]-pt1[0]))?1:0; // which axis to work with
1339 a1=(end1[i1]-pt1[i1])/(pt2[i1]-pt1[i1]);
1340 a2=(end2[i1]-pt1[i1])/(pt2[i1]-pt1[i1]);
1341
1342 if(a2>a1) { // exit through pt2 end
1343 exitpt[0]=end2[0];
1344 exitpt[1]=end2[1];
1345 a=a2;
1346 *exitend=2; }
1347 else { // exit through pt1 end
1348 exitpt[0]=end1[0];
1349 exitpt[1]=end1[1];
1350 a=a1;
1351 *exitend=1; }
1352
1353 return a; }
1354
1355
Geo_LineExitArc2(double * pt1,double * pt2,double * cent,double radius,double * norm,double * exitpt,int * exitend)1356 void Geo_LineExitArc2(double *pt1,double *pt2,double *cent,double radius,double *norm,double *exitpt,int *exitend) {
1357 double zcross;
1358
1359 zcross=(pt1[0]-cent[0])*(pt2[1]-pt1[1])-(pt1[1]-cent[1])*(pt2[0]-pt1[0]);
1360
1361 if(zcross<0) {
1362 exitpt[0]=cent[0]-radius*norm[1];
1363 exitpt[1]=cent[1]+radius*norm[0];
1364 *exitend=1; }
1365 else {
1366 exitpt[0]=cent[0]+radius*norm[1];
1367 exitpt[1]=cent[1]-radius*norm[0];
1368 *exitend=2; }
1369
1370 return; }
1371
1372
Geo_LineExitTriangle(double * pt1,double * pt2,double * normal,double * v1,double * v2,double * v3,double * exitpt,int * exitside)1373 double Geo_LineExitTriangle(double *pt1,double *pt2,double *normal,double *v1,double *v2,double *v3,double *exitpt,int *exitside) {
1374 double edge12[3],edge23[3],edge31[3],line[3],outnorm12[3],outnorm23[3],outnorm31[3],dot12,dot23,dot31;
1375 double a,a12,a23,a31,exitpt12[3],exitpt23[3],exitpt31[3],*exitptr;
1376
1377 edge12[0]=v2[0]-v1[0];
1378 edge12[1]=v2[1]-v1[1];
1379 edge12[2]=v2[2]-v1[2];
1380 edge23[0]=v3[0]-v2[0];
1381 edge23[1]=v3[1]-v2[1];
1382 edge23[2]=v3[2]-v2[2];
1383 edge31[0]=v1[0]-v3[0];
1384 edge31[1]=v1[1]-v3[1];
1385 edge31[2]=v1[2]-v3[2];
1386
1387 line[0]=pt2[0]-pt1[0];
1388 line[1]=pt2[1]-pt1[1];
1389 line[2]=pt2[2]-pt1[2];
1390
1391 outnorm12[0]=edge12[1]*normal[2]-edge12[2]*normal[1];
1392 outnorm12[1]=edge12[2]*normal[0]-edge12[0]*normal[2];
1393 outnorm12[2]=edge12[0]*normal[1]-edge12[1]*normal[0];
1394 dot12=outnorm12[0]*line[0]+outnorm12[1]*line[1]+outnorm12[2]*line[2];
1395
1396 outnorm23[0]=edge23[1]*normal[2]-edge23[2]*normal[1];
1397 outnorm23[1]=edge23[2]*normal[0]-edge23[0]*normal[2];
1398 outnorm23[2]=edge23[0]*normal[1]-edge23[1]*normal[0];
1399 dot23=outnorm23[0]*line[0]+outnorm23[1]*line[1]+outnorm23[2]*line[2];
1400
1401 outnorm31[0]=edge31[1]*normal[2]-edge31[2]*normal[1];
1402 outnorm31[1]=edge31[2]*normal[0]-edge31[0]*normal[2];
1403 outnorm31[2]=edge31[0]*normal[1]-edge31[1]*normal[0];
1404 dot31=outnorm31[0]*line[0]+outnorm31[1]*line[1]+outnorm31[2]*line[2];
1405
1406 a12=(dot12>0)?Geo_LineXPlane(pt1,pt2,v1,outnorm12,exitpt12):INFINITY;
1407 a23=(dot23>0)?Geo_LineXPlane(pt1,pt2,v2,outnorm23,exitpt23):INFINITY;
1408 a31=(dot31>0)?Geo_LineXPlane(pt1,pt2,v3,outnorm31,exitpt31):INFINITY;
1409
1410 if(a12<a23) {
1411 a=a12;
1412 exitptr=exitpt12;
1413 *exitside=1; }
1414 else {
1415 a=a23;
1416 exitptr=exitpt23;
1417 *exitside=2; }
1418 if(a31<a) {
1419 a=a31;
1420 exitptr=exitpt31;
1421 *exitside=3; }
1422
1423 exitpt[0]=exitptr[0];
1424 exitpt[1]=exitptr[1];
1425 exitpt[2]=exitptr[2];
1426
1427 return a; }
1428
1429
Geo_LineExitTriangle2(double * pt1,double * pt2,double ** point,double * exitpt,int * exitside)1430 double Geo_LineExitTriangle2(double *pt1,double *pt2,double **point,double *exitpt,int *exitside) {
1431 double *v1,*v2,*v3,*outnorm12,*outnorm23,*outnorm31,line[3];
1432 double dot12,dot23,dot31,a12,a23,a31,a;
1433
1434 v1=point[0]; // v1 to v3 are triangle vertices
1435 v2=point[1];
1436 v3=point[2];
1437 outnorm12=point[3]; // outnorms are the outward pointing normals of the edges
1438 outnorm23=point[4];
1439 outnorm31=point[5];
1440
1441 line[0]=pt2[0]-pt1[0]; // line is the vector from pt1 to pt2
1442 line[1]=pt2[1]-pt1[1];
1443 line[2]=pt2[2]-pt1[2];
1444
1445 dot12=outnorm12[0]*line[0]+outnorm12[1]*line[1]+outnorm12[2]*line[2]; // >0 if line leaves through this edge
1446 dot23=outnorm23[0]*line[0]+outnorm23[1]*line[1]+outnorm23[2]*line[2];
1447 dot31=outnorm31[0]*line[0]+outnorm31[1]*line[1]+outnorm31[2]*line[2];
1448
1449 a12=(dot12>0)?((v1[0]-pt1[0])*outnorm12[0]+(v1[1]-pt1[1])*outnorm12[1]+(v1[2]-pt1[2])*outnorm12[2])/dot12:INFINITY;
1450 a23=(dot23>0)?((v2[0]-pt1[0])*outnorm23[0]+(v2[1]-pt1[1])*outnorm23[1]+(v2[2]-pt1[2])*outnorm23[2])/dot23:INFINITY;
1451 a31=(dot31>0)?((v3[0]-pt1[0])*outnorm31[0]+(v3[1]-pt1[1])*outnorm31[1]+(v3[2]-pt1[2])*outnorm31[2])/dot31:INFINITY;
1452
1453 if(a12<a23) {
1454 a=a12;
1455 *exitside=1; }
1456 else {
1457 a=a23;
1458 *exitside=2; }
1459 if(a31<a) {
1460 a=a31;
1461 *exitside=3; }
1462
1463 exitpt[0]=pt1[0]+a*line[0];
1464 exitpt[1]=pt1[1]+a*line[1];
1465 exitpt[2]=pt1[2]+a*line[2];
1466
1467 return a; }
1468
1469
Geo_LineExitSphere(double * pt1,double * pt2,double * cent,double rad,double * exitpt)1470 double Geo_LineExitSphere(double *pt1,double *pt2,double *cent,double rad,double *exitpt) {
1471 double a,b,c,pt12,ptc1,cross,radical;
1472
1473 a=b=c=0;
1474 pt12=pt2[0]-pt1[0];
1475 ptc1=cent[0]-pt1[0];
1476 a+=pt12*pt12;
1477 b+=ptc1*pt12;
1478 c+=ptc1*ptc1;
1479
1480 pt12=pt2[1]-pt1[1];
1481 ptc1=cent[1]-pt1[1];
1482 a+=pt12*pt12;
1483 b+=ptc1*pt12;
1484 c+=ptc1*ptc1;
1485
1486 pt12=pt2[2]-pt1[2];
1487 ptc1=cent[2]-pt1[2];
1488 a+=pt12*pt12;
1489 b+=ptc1*pt12;
1490 c+=ptc1*ptc1;
1491
1492 b*=-2;
1493 c-=rad*rad;
1494 radical=b*b-4*a*c;
1495 radical=(radical<=0)?0:sqrt(radical);
1496 cross=(-b+radical)/(2*a);
1497
1498 exitpt[0]=pt1[0]+cross*(pt2[0]-pt1[0]);
1499 exitpt[1]=pt1[1]+cross*(pt2[1]-pt1[1]);
1500 exitpt[2]=pt1[2]+cross*(pt2[2]-pt1[2]);
1501
1502 return cross; }
1503
1504
Geo_LineExitHemisphere(double * pt1,double * pt2,double * cent,double rad,double * normal,double * exitpt)1505 void Geo_LineExitHemisphere(double *pt1,double *pt2,double *cent,double rad,double *normal,double *exitpt) {
1506 double crsspt[3],vect[3];
1507
1508 Geo_LineXPlane(pt1,pt2,cent,normal,crsspt); // find point where line crosses hemisphere plane
1509 Geo_SphereNormal(cent,crsspt,1,3,vect); // find sphere normal that goes through crsspt
1510 exitpt[0]=cent[0]+rad*vect[0];
1511 exitpt[1]=cent[1]+rad*vect[1];
1512 exitpt[2]=cent[2]+rad*vect[2];
1513
1514 return; }
1515
1516
Geo_LineExitCylinder(double * pt1,double * pt2,double * end1,double * end2,double rad,double * exitpt,int * exitend)1517 double Geo_LineExitCylinder(double *pt1,double *pt2,double *end1,double *end2,double rad,double *exitpt,int *exitend) {
1518 double axis[3],dot,*exitcenter,a,crsspt[3];
1519
1520 axis[0]=end2[0]-end1[0]; // vector for the cylinder axis
1521 axis[1]=end2[1]-end1[1];
1522 axis[2]=end2[2]-end1[2];
1523
1524 dot=(pt2[0]-pt1[0])*axis[0]+(pt2[1]-pt1[1])*axis[1]+(pt2[2]-pt1[2])*axis[2]; // dot product of the line with the cylinder axis
1525 if(dot>0) {
1526 exitcenter=end2;
1527 *exitend=2; }
1528 else {
1529 exitcenter=end1;
1530 *exitend=1; }
1531
1532 a=Geo_LineXPlane(pt1,pt2,exitcenter,axis,crsspt);
1533 Geo_NearestCylinderPt(end1,end2,rad,3,crsspt,exitpt,0);
1534
1535 return a; }
1536
1537
1538 /******************* ...Xaabb ****************/
1539
Geo_LineXaabb2(double * pt1,double * pt2,double * norm,double * bpt1,double * bpt2)1540 int Geo_LineXaabb2(double *pt1,double *pt2,double *norm,double *bpt1,double *bpt2) {
1541 double cmp,b1,b2,b3,b4;
1542
1543 if(pt1[0]<bpt1[0] && pt2[0]<bpt1[0]) return 0;
1544 if(pt1[0]>bpt2[0] && pt2[0]>bpt2[0]) return 0;
1545 if(pt1[1]<bpt1[1] && pt2[1]<bpt1[1]) return 0;
1546 if(pt1[1]>bpt2[1] && pt2[1]>bpt2[1]) return 0;
1547 cmp=norm[0]*pt1[0]+norm[1]*pt1[1];
1548 b1=norm[0]*bpt1[0]+norm[1]*bpt1[1];
1549 b2=norm[0]*bpt1[0]+norm[1]*bpt2[1];
1550 b3=norm[0]*bpt2[0]+norm[1]*bpt1[1];
1551 b4=norm[0]*bpt2[0]+norm[1]*bpt2[1];
1552 if(b1<cmp && b2<cmp && b3<cmp && b4<cmp) return 0;
1553 if(b1>cmp && b2>cmp && b3>cmp && b4>cmp) return 0;
1554 return 1; }
1555
1556
Geo_LineXaabb(double * pt1,double * pt2,double * bpt1,double * bpt2,int dim,int infline)1557 int Geo_LineXaabb(double *pt1,double *pt2,double *bpt1,double *bpt2,int dim,int infline) {
1558 int d;
1559 double nearhi,farlo,denom,a1,a2;
1560
1561 nearhi=-1.0;
1562 farlo=2.0;
1563 for(d=0;d<dim;d++) {
1564 denom=pt2[d]-pt1[d];
1565 if(denom==0) {
1566 if(pt1[d]<bpt1[d] || pt1[d]>bpt2[d]) return 0; }
1567 else {
1568 a1=(bpt1[d]-pt1[d])/denom;
1569 a2=(bpt2[d]-pt1[d])/denom;
1570 if(a1<a2) {
1571 if(a1>nearhi) nearhi=a1;
1572 if(a2<farlo) farlo=a2; }
1573 else {
1574 if(a2>nearhi) nearhi=a2;
1575 if(a1<farlo) farlo=a1; }}}
1576
1577 if(infline) return(nearhi<=farlo);
1578 else return(nearhi<=farlo && nearhi<=1 && farlo>=0); }
1579
1580
Geo_TriXaabb3(double * pt1,double * pt2,double * pt3,double * norm,double * bpt1,double * bpt2)1581 int Geo_TriXaabb3(double *pt1,double *pt2,double *pt3,double *norm,double *bpt1,double *bpt2) {
1582 double cmp,b1,b2,b3,b4,b5,b6,b7,b8;
1583
1584 if(pt1[0]<bpt1[0] && pt2[0]<bpt1[0] && pt3[0]<bpt1[0]) return 0;
1585 if(pt1[0]>bpt2[0] && pt2[0]>bpt2[0] && pt3[0]>bpt2[0]) return 0;
1586 if(pt1[1]<bpt1[1] && pt2[1]<bpt1[1] && pt3[1]<bpt1[1]) return 0;
1587 if(pt1[1]>bpt2[1] && pt2[1]>bpt2[1] && pt3[1]>bpt2[1]) return 0;
1588 if(pt1[2]<bpt1[2] && pt2[2]<bpt1[2] && pt3[2]<bpt1[2]) return 0;
1589 if(pt1[2]>bpt2[2] && pt2[2]>bpt2[2] && pt3[2]>bpt2[2]) return 0;
1590 cmp=norm[0]*pt1[0]+norm[1]*pt1[1]+norm[2]*pt1[2];
1591 b1=norm[0]*bpt1[0]+norm[1]*bpt1[1]+norm[2]*bpt1[2];
1592 b2=norm[0]*bpt1[0]+norm[1]*bpt1[1]+norm[2]*bpt2[2];
1593 b3=norm[0]*bpt1[0]+norm[1]*bpt2[1]+norm[2]*bpt1[2];
1594 b4=norm[0]*bpt1[0]+norm[1]*bpt2[1]+norm[2]*bpt2[2];
1595 b5=norm[0]*bpt2[0]+norm[1]*bpt1[1]+norm[2]*bpt1[2];
1596 b6=norm[0]*bpt2[0]+norm[1]*bpt1[1]+norm[2]*bpt2[2];
1597 b7=norm[0]*bpt2[0]+norm[1]*bpt2[1]+norm[2]*bpt1[2];
1598 b8=norm[0]*bpt2[0]+norm[1]*bpt2[1]+norm[2]*bpt2[2];
1599 if(b1<cmp && b2<cmp && b3<cmp && b4<cmp && b5<cmp && b6<cmp && b7<cmp && b8<cmp) return 0;
1600 if(b1>cmp && b2>cmp && b3>cmp && b4>cmp && b5>cmp && b6>cmp && b7>cmp && b8>cmp) return 0;
1601 return 1; }
1602
1603
Geo_RectXaabb2(double * r1,double * r2,double * r3,double * bpt1,double * bpt2)1604 int Geo_RectXaabb2(double *r1,double *r2,double *r3,double *bpt1,double *bpt2) {
1605 double v1[2],cmp,b1,b2,b3,b4;
1606
1607 v1[0]=r2[0]+r3[0]-r1[0]; // point 4
1608 v1[1]=r2[1]+r3[1]-r1[1];
1609 if(r1[0]<bpt1[0] && r2[0]<bpt1[0] && r3[0]<bpt1[0] && v1[0]<bpt1[0]) return 0;
1610 if(r1[0]>bpt2[0] && r2[0]>bpt2[0] && r3[0]>bpt2[0] && v1[0]>bpt2[0]) return 0;
1611 if(r1[1]<bpt1[1] && r2[1]<bpt1[1] && r3[1]<bpt1[1] && v1[1]<bpt1[1]) return 0;
1612 if(r1[1]>bpt2[1] && r2[1]>bpt2[1] && r3[1]>bpt2[1] && v1[1]>bpt2[1]) return 0;
1613 v1[0]=r2[0]-r1[0]; // side 1,2
1614 v1[1]=r2[1]-r1[1];
1615 cmp=v1[0]*r1[0]+v1[1]*r1[1];
1616 b1=v1[0]*bpt1[0]+v1[1]*bpt1[1];
1617 b2=v1[0]*bpt1[0]+v1[1]*bpt2[1];
1618 b3=v1[0]*bpt2[0]+v1[1]*bpt1[1];
1619 b4=v1[0]*bpt2[0]+v1[1]*bpt2[1];
1620 if(b1<cmp && b2<cmp && b3<cmp && b4<cmp) return 0;
1621 cmp=v1[0]*r2[0]+v1[1]*r2[1];
1622 if(b1>cmp && b2>cmp && b3>cmp && b4>cmp) return 0;
1623 v1[0]=r3[0]-r1[0]; // side 2,3
1624 v1[1]=r3[1]-r1[1];
1625 cmp=v1[0]*r1[0]+v1[1]*r1[1];
1626 b1=v1[0]*bpt1[0]+v1[1]*bpt1[1];
1627 b2=v1[0]*bpt1[0]+v1[1]*bpt2[1];
1628 b3=v1[0]*bpt2[0]+v1[1]*bpt1[1];
1629 b4=v1[0]*bpt2[0]+v1[1]*bpt2[1];
1630 if(b1<cmp && b2<cmp && b3<cmp && b4<cmp) return 0;
1631 cmp=v1[0]*r3[0]+v1[1]*r3[1];
1632 if(b1>cmp && b2>cmp && b3>cmp && b4>cmp) return 0;
1633 return 1; }
1634
1635
Geo_RectXaabb3(double * r1,double * r2,double * r3,double * r4,double * bpt1,double * bpt2)1636 int Geo_RectXaabb3(double *r1,double *r2,double *r3,double *r4,double *bpt1,double *bpt2) {
1637 double r5[3],r6[3],r7[3],v1[3],b1,b2,b3,b4,b5,b6,b7,b8,cmp;
1638
1639 r5[0]=r2[0]+r4[0]-r1[0];
1640 r5[1]=r2[1]+r4[1]-r1[1];
1641 r5[2]=r2[2]+r4[2]-r1[2];
1642 r6[0]=r3[0]+r4[0]-r1[0];
1643 r6[1]=r3[1]+r4[1]-r1[1];
1644 r6[2]=r3[2]+r4[2]-r1[2];
1645 r7[0]=r2[0]+r3[0]-r1[0];
1646 r7[1]=r2[1]+r3[1]-r1[1];
1647 r7[2]=r2[2]+r3[2]-r1[2];
1648 //r8 not used
1649 //r8[0]=r7[0]+r4[0]-r1[0];
1650 //r8[1]=r7[1]+r4[1]-r1[1];
1651 //r8[2]=r7[2]+r4[2]-r1[2];
1652 if(r1[0]<bpt1[0] && r2[0]<bpt1[0] && r3[0]<bpt1[0] && r4[0]<bpt1[0] && r5[0]<bpt1[0] && r6[0]<bpt1[0] && r7[0]<bpt1[0]) return 0;
1653 if(r1[0]>bpt2[0] && r2[0]>bpt2[0] && r3[0]>bpt2[0] && r4[0]>bpt2[0] && r5[0]>bpt2[0] && r6[0]>bpt2[0] && r7[0]>bpt2[0]) return 0;
1654 if(r1[1]<bpt1[1] && r2[1]<bpt1[1] && r3[1]<bpt1[1] && r4[1]<bpt1[1] && r5[1]<bpt1[1] && r6[1]<bpt1[1] && r7[1]<bpt1[1]) return 0;
1655 if(r1[1]>bpt2[1] && r2[1]>bpt2[1] && r3[1]>bpt2[1] && r4[1]>bpt2[1] && r5[1]>bpt2[1] && r6[1]>bpt2[1] && r7[1]>bpt2[1]) return 0;
1656 if(r1[2]<bpt1[2] && r2[2]<bpt1[2] && r3[2]<bpt1[2] && r4[2]<bpt1[2] && r5[2]<bpt1[2] && r6[2]<bpt1[2] && r7[2]<bpt1[2]) return 0;
1657 if(r1[2]>bpt2[2] && r2[2]>bpt2[2] && r3[2]>bpt2[2] && r4[2]>bpt2[2] && r5[2]>bpt2[2] && r6[2]>bpt2[2] && r7[2]>bpt2[2]) return 0;
1658 v1[0]=r2[0]-r1[0]; // side 1,2
1659 v1[1]=r2[1]-r1[1];
1660 v1[2]=r2[2]-r1[2];
1661 cmp=v1[0]*r1[0]+v1[1]*r1[1]+v1[2]*r1[2];
1662 b1=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];
1663 b2=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
1664 b3=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
1665 b4=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
1666 b5=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];
1667 b6=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
1668 b7=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
1669 b8=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
1670 if(b1<cmp && b2<cmp && b3<cmp && b4<cmp && b5<cmp && b6<cmp && b7<cmp && b8<cmp) return 0;
1671 cmp=v1[0]*r2[0]+v1[1]*r2[1]+v1[2]*r2[2];
1672 if(b1>cmp && b2>cmp && b3>cmp && b4>cmp && b5>cmp && b6>cmp && b7>cmp && b8>cmp) return 0;
1673 v1[0]=r3[0]-r1[0]; // side 1,3
1674 v1[1]=r3[1]-r1[1];
1675 v1[2]=r3[2]-r1[2];
1676 cmp=v1[0]*r1[0]+v1[1]*r1[1]+v1[2]*r1[2];
1677 b1=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];
1678 b2=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
1679 b3=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
1680 b4=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
1681 b5=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];
1682 b6=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
1683 b7=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
1684 b8=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
1685 if(b1<cmp && b2<cmp && b3<cmp && b4<cmp && b5<cmp && b6<cmp && b7<cmp && b8<cmp) return 0;
1686 cmp=v1[0]*r3[0]+v1[1]*r3[1]+v1[2]*r3[2];
1687 if(b1>cmp && b2>cmp && b3>cmp && b4>cmp && b5>cmp && b6>cmp && b7>cmp && b8>cmp) return 0;
1688 v1[0]=r4[0]-r1[0]; // side 1,4
1689 v1[1]=r4[1]-r1[1];
1690 v1[2]=r4[2]-r1[2];
1691 cmp=v1[0]*r1[0]+v1[1]*r1[1]+v1[2]*r1[2];
1692 b1=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];
1693 b2=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
1694 b3=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
1695 b4=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
1696 b5=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];
1697 b6=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
1698 b7=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
1699 b8=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
1700 if(b1<cmp && b2<cmp && b3<cmp && b4<cmp && b5<cmp && b6<cmp && b7<cmp && b8<cmp) return 0;
1701 cmp=v1[0]*r4[0]+v1[1]*r4[1]+v1[2]*r4[2];
1702 if(b1>cmp && b2>cmp && b3>cmp && b4>cmp && b5>cmp && b6>cmp && b7>cmp && b8>cmp) return 0;
1703 return 1; }
1704
1705
Geo_CircleXaabb2(double * cent,double rad,double * bpt1,double * bpt2)1706 int Geo_CircleXaabb2(double *cent,double rad,double *bpt1,double *bpt2) {
1707 double min,max,d2,r2;
1708
1709 if(cent[0]+rad<bpt1[0]) return 0; // test projections on x,y
1710 if(cent[0]-rad>bpt2[0]) return 0;
1711 if(cent[1]+rad<bpt1[1]) return 0;
1712 if(cent[1]-rad>bpt2[1]) return 0;
1713 r2=rad*rad; // find closest, farthest corners
1714 d2=(bpt1[0]-cent[0])*(bpt1[0]-cent[0])+(bpt1[1]-cent[1])*(bpt1[1]-cent[1]);
1715 min=max=d2;
1716 d2=(bpt1[0]-cent[0])*(bpt1[0]-cent[0])+(bpt2[1]-cent[1])*(bpt2[1]-cent[1]);
1717 if(d2<min) min=d2;
1718 else if(d2>max) max=d2;
1719 d2=(bpt2[0]-cent[0])*(bpt2[0]-cent[0])+(bpt1[1]-cent[1])*(bpt1[1]-cent[1]);
1720 if(d2<min) min=d2;
1721 else if(d2>max) max=d2;
1722 d2=(bpt2[0]-cent[0])*(bpt2[0]-cent[0])+(bpt2[1]-cent[1])*(bpt2[1]-cent[1]);
1723 if(d2<min) min=d2;
1724 else if(d2>max) max=d2;
1725 if(r2>max) return 0; // box totally inside circle
1726 if(r2>=min) return 1; // at least 1 corner inside, but not all
1727 if(cent[0]>=bpt1[0] && cent[0]<=bpt2[0]) return 1; // circle crosses box side
1728 if(cent[1]>=bpt1[1] && cent[1]<=bpt2[1]) return 1;
1729 return 0; } // circle near box corner, not crossing
1730
1731
Geo_SphsXaabb3(double * cent,double rad,double * bpt1,double * bpt2)1732 int Geo_SphsXaabb3(double *cent,double rad,double *bpt1,double *bpt2) {
1733 double min,max,d2,r2;
1734
1735 if(cent[0]+rad<bpt1[0]) return 0;
1736 if(cent[0]-rad>bpt2[0]) return 0;
1737 if(cent[1]+rad<bpt1[1]) return 0;
1738 if(cent[1]-rad>bpt2[1]) return 0;
1739 if(cent[2]+rad<bpt1[2]) return 0;
1740 if(cent[2]-rad>bpt2[2]) return 0;
1741 r2=rad*rad;
1742 d2=(bpt1[0]-cent[0])*(bpt1[0]-cent[0])+(bpt1[1]-cent[1])*(bpt1[1]-cent[1])+(bpt1[2]-cent[2])*(bpt1[2]-cent[2]);
1743 min=max=d2;
1744 d2=(bpt1[0]-cent[0])*(bpt1[0]-cent[0])+(bpt1[1]-cent[1])*(bpt1[1]-cent[1])+(bpt2[2]-cent[2])*(bpt2[2]-cent[2]);
1745 if(d2<min) min=d2;
1746 else if(d2>max) max=d2;
1747 d2=(bpt1[0]-cent[0])*(bpt1[0]-cent[0])+(bpt2[1]-cent[1])*(bpt2[1]-cent[1])+(bpt1[2]-cent[2])*(bpt1[2]-cent[2]);
1748 if(d2<min) min=d2;
1749 else if(d2>max) max=d2;
1750 d2=(bpt1[0]-cent[0])*(bpt1[0]-cent[0])+(bpt2[1]-cent[1])*(bpt2[1]-cent[1])+(bpt2[2]-cent[2])*(bpt2[2]-cent[2]);
1751 if(d2<min) min=d2;
1752 else if(d2>max) max=d2;
1753 d2=(bpt2[0]-cent[0])*(bpt2[0]-cent[0])+(bpt1[1]-cent[1])*(bpt1[1]-cent[1])+(bpt1[2]-cent[2])*(bpt1[2]-cent[2]);
1754 if(d2<min) min=d2;
1755 else if(d2>max) max=d2;
1756 d2=(bpt2[0]-cent[0])*(bpt2[0]-cent[0])+(bpt1[1]-cent[1])*(bpt1[1]-cent[1])+(bpt2[2]-cent[2])*(bpt2[2]-cent[2]);
1757 if(d2<min) min=d2;
1758 else if(d2>max) max=d2;
1759 d2=(bpt2[0]-cent[0])*(bpt2[0]-cent[0])+(bpt2[1]-cent[1])*(bpt2[1]-cent[1])+(bpt1[2]-cent[2])*(bpt1[2]-cent[2]);
1760 if(d2<min) min=d2;
1761 else if(d2>max) max=d2;
1762 d2=(bpt2[0]-cent[0])*(bpt2[0]-cent[0])+(bpt2[1]-cent[1])*(bpt2[1]-cent[1])+(bpt2[2]-cent[2])*(bpt2[2]-cent[2]);
1763 if(d2<min) min=d2;
1764 else if(d2>max) max=d2;
1765 if(r2>max) return 0;
1766 if(r2>=min) return 1;
1767 if(cent[0]>=bpt1[0] && cent[0]<=bpt2[0]) return 1;
1768 if(cent[1]>=bpt1[1] && cent[1]<=bpt2[1]) return 1;
1769 if(cent[2]>=bpt1[2] && cent[2]<=bpt2[2]) return 1;
1770 return 1; }
1771
1772
Geo_CylisXaabb3(double * pt1,double * pt2,double rad,double * bpt1,double * bpt2)1773 int Geo_CylisXaabb3(double *pt1,double *pt2,double rad,double *bpt1,double *bpt2) {
1774 double v1[3],v2[3],v3[3];
1775 double cent[2],ab1[2],ab2[2],ab3[2],ab4[2],ab5[2],ab6[2],ab7[2],ab8[2];
1776 double nrm,b1,b2,b3,b4,b5,b6,b7,b8,cmp,r2,d2,min,max,crss1,crss2;
1777
1778 Geo_LineNormal3D(pt1,pt2,pt1,v1); // v1 and v2 are normalized and perp. to cyl. axis
1779 v3[0]=pt2[0]-pt1[0];
1780 v3[1]=pt2[1]-pt1[1];
1781 v3[2]=pt2[2]-pt1[2];
1782 nrm=sqrt(v3[0]*v3[0]+v3[1]*v3[1]+v3[2]*v3[2]);
1783 if(!nrm) return 0;
1784 v3[0]/=nrm; // v3 is unit vector along cyl. axis, only used here
1785 v3[1]/=nrm;
1786 v3[2]/=nrm;
1787 v2[0]=v3[1]*v1[2]-v3[2]*v1[1]; // v2 = v3 x v1
1788 v2[1]=v3[2]*v1[0]-v3[0]*v1[2];
1789 v2[2]=v3[0]*v1[1]-v3[1]*v1[0];
1790
1791 cent[0]=v1[0]*pt1[0]+v1[1]*pt1[1]+v1[2]*pt1[2]; // cent is the center of the circle on v1,v2 axes
1792 cent[1]=v2[0]*pt1[0]+v2[1]*pt1[1]+v2[2]*pt1[2];
1793
1794 ab1[0]=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2]; // ab are box pts in 2-D
1795 ab1[1]=v2[0]*bpt1[0]+v2[1]*bpt1[1]+v2[2]*bpt1[2];
1796 ab2[0]=v1[0]*bpt1[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
1797 ab2[1]=v2[0]*bpt1[0]+v2[1]*bpt1[1]+v2[2]*bpt2[2];
1798 ab3[0]=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
1799 ab3[1]=v2[0]*bpt1[0]+v2[1]*bpt2[1]+v2[2]*bpt1[2];
1800 ab4[0]=v1[0]*bpt1[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
1801 ab4[1]=v2[0]*bpt1[0]+v2[1]*bpt2[1]+v2[2]*bpt2[2];
1802 ab5[0]=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt1[2];
1803 ab5[1]=v2[0]*bpt2[0]+v2[1]*bpt1[1]+v2[2]*bpt1[2];
1804 ab6[0]=v1[0]*bpt2[0]+v1[1]*bpt1[1]+v1[2]*bpt2[2];
1805 ab6[1]=v2[0]*bpt2[0]+v2[1]*bpt1[1]+v2[2]*bpt2[2];
1806 ab7[0]=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt1[2];
1807 ab7[1]=v2[0]*bpt2[0]+v2[1]*bpt2[1]+v2[2]*bpt1[2];
1808 ab8[0]=v1[0]*bpt2[0]+v1[1]*bpt2[1]+v1[2]*bpt2[2];
1809 ab8[1]=v2[0]*bpt2[0]+v2[1]*bpt2[1]+v2[2]*bpt2[2];
1810
1811 v1[0]=ab2[0]-ab1[0]; // Test1: do SAT on the three box axes
1812 v1[1]=ab2[1]-ab1[1];
1813 nrm=sqrt(v1[0]*v1[0]+v1[1]*v1[1]);
1814 if(nrm>0) {
1815 v1[0]/=nrm;
1816 v1[1]/=nrm;
1817 b1=v1[0]*ab1[0]+v1[1]*ab1[1];
1818 b2=v1[0]*ab2[0]+v1[1]*ab2[1];
1819 b3=v1[0]*ab3[0]+v1[1]*ab3[1];
1820 b4=v1[0]*ab4[0]+v1[1]*ab4[1];
1821 b5=v1[0]*ab5[0]+v1[1]*ab5[1];
1822 b6=v1[0]*ab6[0]+v1[1]*ab6[1];
1823 b7=v1[0]*ab7[0]+v1[1]*ab7[1];
1824 b8=v1[0]*ab8[0]+v1[1]*ab8[1];
1825 cmp=v1[0]*cent[0]+v1[1]*cent[1]-rad;
1826 if(b1<cmp && b2<cmp && b3<cmp && b4<cmp && b5<cmp && b6<cmp && b7<cmp && b8<cmp) return 0;
1827 cmp=v1[0]*cent[0]+v1[1]*cent[1]+rad;
1828 if(b1>cmp && b2>cmp && b3>cmp && b4>cmp && b5>cmp && b6>cmp && b7>cmp && b8>cmp) return 0; }
1829
1830 v1[0]=ab3[0]-ab1[0];
1831 v1[1]=ab3[1]-ab1[1];
1832 nrm=sqrt(v1[0]*v1[0]+v1[1]*v1[1]);
1833 if(nrm>0) {
1834 v1[0]/=nrm;
1835 v1[1]/=nrm;
1836 b1=v1[0]*ab1[0]+v1[1]*ab1[1];
1837 b2=v1[0]*ab2[0]+v1[1]*ab2[1];
1838 b3=v1[0]*ab3[0]+v1[1]*ab3[1];
1839 b4=v1[0]*ab4[0]+v1[1]*ab4[1];
1840 b5=v1[0]*ab5[0]+v1[1]*ab5[1];
1841 b6=v1[0]*ab6[0]+v1[1]*ab6[1];
1842 b7=v1[0]*ab7[0]+v1[1]*ab7[1];
1843 b8=v1[0]*ab8[0]+v1[1]*ab8[1];
1844 cmp=v1[0]*cent[0]+v1[1]*cent[1]-rad;
1845 if(b1<cmp && b2<cmp && b3<cmp && b4<cmp && b5<cmp && b6<cmp && b7<cmp && b8<cmp) return 0;
1846 cmp=v1[0]*cent[0]+v1[1]*cent[1]+rad;
1847 if(b1>cmp && b2>cmp && b3>cmp && b4>cmp && b5>cmp && b6>cmp && b7>cmp && b8>cmp) return 0; }
1848
1849 v1[0]=ab5[0]-ab1[0];
1850 v1[1]=ab5[1]-ab1[1];
1851 nrm=sqrt(v1[0]*v1[0]+v1[1]*v1[1]);
1852 if(nrm>0) {
1853 v1[0]/=nrm;
1854 v1[1]/=nrm;
1855 b1=v1[0]*ab1[0]+v1[1]*ab1[1];
1856 b2=v1[0]*ab2[0]+v1[1]*ab2[1];
1857 b3=v1[0]*ab3[0]+v1[1]*ab3[1];
1858 b4=v1[0]*ab4[0]+v1[1]*ab4[1];
1859 b5=v1[0]*ab5[0]+v1[1]*ab5[1];
1860 b6=v1[0]*ab6[0]+v1[1]*ab6[1];
1861 b7=v1[0]*ab7[0]+v1[1]*ab7[1];
1862 b8=v1[0]*ab8[0]+v1[1]*ab8[1];
1863 cmp=v1[0]*cent[0]+v1[1]*cent[1]-rad;
1864 if(b1<cmp && b2<cmp && b3<cmp && b4<cmp && b5<cmp && b6<cmp && b7<cmp && b8<cmp) return 0;
1865 cmp=v1[0]*cent[0]+v1[1]*cent[1]+rad;
1866 if(b1>cmp && b2>cmp && b3>cmp && b4>cmp && b5>cmp && b6>cmp && b7>cmp && b8>cmp) return 0; }
1867
1868 r2=rad*rad; // Tests 2,3: check corners inside circle
1869 d2=(ab1[0]-cent[0])*(ab1[0]-cent[0])+(ab1[1]-cent[1])*(ab1[1]-cent[1]);
1870 min=max=d2;
1871 d2=(ab2[0]-cent[0])*(ab2[0]-cent[0])+(ab2[1]-cent[1])*(ab2[1]-cent[1]);
1872 if(d2<min) min=d2;
1873 else if(d2>max) max=d2;
1874 d2=(ab3[0]-cent[0])*(ab3[0]-cent[0])+(ab3[1]-cent[1])*(ab3[1]-cent[1]);
1875 if(d2<min) min=d2;
1876 else if(d2>max) max=d2;
1877 d2=(ab4[0]-cent[0])*(ab4[0]-cent[0])+(ab4[1]-cent[1])*(ab4[1]-cent[1]);
1878 if(d2<min) min=d2;
1879 else if(d2>max) max=d2;
1880 d2=(ab5[0]-cent[0])*(ab5[0]-cent[0])+(ab5[1]-cent[1])*(ab5[1]-cent[1]);
1881 if(d2<min) min=d2;
1882 else if(d2>max) max=d2;
1883 d2=(ab6[0]-cent[0])*(ab6[0]-cent[0])+(ab6[1]-cent[1])*(ab6[1]-cent[1]);
1884 if(d2<min) min=d2;
1885 else if(d2>max) max=d2;
1886 d2=(ab7[0]-cent[0])*(ab7[0]-cent[0])+(ab7[1]-cent[1])*(ab7[1]-cent[1]);
1887 if(d2<min) min=d2;
1888 else if(d2>max) max=d2;
1889 d2=(ab8[0]-cent[0])*(ab8[0]-cent[0])+(ab8[1]-cent[1])*(ab8[1]-cent[1]);
1890 if(d2<min) min=d2;
1891 else if(d2>max) max=d2;
1892 if(r2>max) return 0;
1893 if(r2>=min) return 1;
1894
1895 if(Geo_LineXaabb(pt1,pt2,bpt1,bpt2,3,1)) return 1; // Test 4: check cylinder center crosses aabb
1896
1897 // Test 5: check edge crossing
1898 if(((crss1=Geo_LineXSphs(ab1,ab2,cent,rad,2,&crss2,NULL,NULL))>=0 && crss1<=1) || (crss2>=0 && crss2<=1)) return 1;
1899 if(((crss1=Geo_LineXSphs(ab2,ab4,cent,rad,2,&crss2,NULL,NULL))>=0 && crss1<=1) || (crss2>=0 && crss2<=1)) return 1;
1900 if(((crss1=Geo_LineXSphs(ab4,ab3,cent,rad,2,&crss2,NULL,NULL))>=0 && crss1<=1) || (crss2>=0 && crss2<=1)) return 1;
1901 if(((crss1=Geo_LineXSphs(ab3,ab1,cent,rad,2,&crss2,NULL,NULL))>=0 && crss1<=1) || (crss2>=0 && crss2<=1)) return 1;
1902 if(((crss1=Geo_LineXSphs(ab1,ab5,cent,rad,2,&crss2,NULL,NULL))>=0 && crss1<=1) || (crss2>=0 && crss2<=1)) return 1;
1903 if(((crss1=Geo_LineXSphs(ab2,ab6,cent,rad,2,&crss2,NULL,NULL))>=0 && crss1<=1) || (crss2>=0 && crss2<=1)) return 1;
1904 if(((crss1=Geo_LineXSphs(ab3,ab7,cent,rad,2,&crss2,NULL,NULL))>=0 && crss1<=1) || (crss2>=0 && crss2<=1)) return 1;
1905 if(((crss1=Geo_LineXSphs(ab4,ab8,cent,rad,2,&crss2,NULL,NULL))>=0 && crss1<=1) || (crss2>=0 && crss2<=1)) return 1;
1906 if(((crss1=Geo_LineXSphs(ab5,ab6,cent,rad,2,&crss2,NULL,NULL))>=0 && crss1<=1) || (crss2>=0 && crss2<=1)) return 1;
1907 if(((crss1=Geo_LineXSphs(ab6,ab8,cent,rad,2,&crss2,NULL,NULL))>=0 && crss1<=1) || (crss2>=0 && crss2<=1)) return 1;
1908 if(((crss1=Geo_LineXSphs(ab8,ab7,cent,rad,2,&crss2,NULL,NULL))>=0 && crss1<=1) || (crss2>=0 && crss2<=1)) return 1;
1909 if(((crss1=Geo_LineXSphs(ab7,ab5,cent,rad,2,&crss2,NULL,NULL))>=0 && crss1<=1) || (crss2>=0 && crss2<=1)) return 1;
1910
1911 return 0; }
1912
1913
Geo_DiskXaabb3(double * cent,double rad,double * norm,double * bpt1,double * bpt2)1914 int Geo_DiskXaabb3(double *cent,double rad,double *norm,double *bpt1,double *bpt2) {
1915 double dot,cmp,b1,b2,b3,b4,b5,b6,b7,b8;
1916
1917 dot=sqrt(norm[1]*norm[1]+norm[2]*norm[2]); // SAT with aabb faces
1918 if(cent[0]-rad*dot>bpt2[0]) return 0;
1919 if(cent[0]+rad*dot<bpt1[0]) return 0;
1920 dot=sqrt(norm[0]*norm[0]+norm[2]*norm[2]);
1921 if(cent[1]-rad*dot>bpt2[1]) return 0;
1922 if(cent[1]+rad*dot<bpt1[1]) return 0;
1923 dot=sqrt(norm[0]*norm[0]+norm[1]*norm[1]);
1924 if(cent[2]-rad*dot>bpt2[2]) return 0;
1925 if(cent[2]+rad*dot<bpt1[2]) return 0;
1926
1927 cmp=norm[0]*cent[0]+norm[1]*cent[1]+norm[2]*cent[2]; // SAT with disk face
1928 b1=norm[0]*bpt1[0]+norm[1]*bpt1[1]+norm[2]*bpt1[2];
1929 b2=norm[0]*bpt1[0]+norm[1]*bpt1[1]+norm[2]*bpt2[2];
1930 b3=norm[0]*bpt1[0]+norm[1]*bpt2[1]+norm[2]*bpt1[2];
1931 b4=norm[0]*bpt1[0]+norm[1]*bpt2[1]+norm[2]*bpt2[2];
1932 b5=norm[0]*bpt2[0]+norm[1]*bpt1[1]+norm[2]*bpt1[2];
1933 b6=norm[0]*bpt2[0]+norm[1]*bpt1[1]+norm[2]*bpt2[2];
1934 b7=norm[0]*bpt2[0]+norm[1]*bpt2[1]+norm[2]*bpt1[2];
1935 b8=norm[0]*bpt2[0]+norm[1]*bpt2[1]+norm[2]*bpt2[2];
1936 if(b1<cmp && b2<cmp && b3<cmp && b4<cmp && b5<cmp && b6<cmp && b7<cmp && b8<cmp) return 0;
1937 if(b1>cmp && b2>cmp && b3>cmp && b4>cmp && b5>cmp && b6>cmp && b7>cmp && b8>cmp) return 0;
1938
1939 return 1; }
1940
1941
1942 /******************** approx. Xaabb ************/
1943
Geo_SemicXaabb2(double * cent,double rad,double * outvect,double * bpt1,double * bpt2)1944 int Geo_SemicXaabb2(double *cent,double rad,double *outvect,double *bpt1,double *bpt2) {
1945 double r1[2],r2[2],r3[2];
1946
1947 if(!Geo_CircleXaabb2(cent,rad,bpt1,bpt2)) return 0;
1948 Geo_Semic2Rect(cent,rad,outvect,r1,r2,r3);
1949 return Geo_RectXaabb2(r1,r2,r3,bpt1,bpt2); }
1950
1951
Geo_HemisXaabb3(double * cent,double rad,double * outvect,double * bpt1,double * bpt2)1952 int Geo_HemisXaabb3(double *cent,double rad,double *outvect,double *bpt1,double *bpt2) {
1953 double r1[3],r2[3],r3[3],r4[3];
1954
1955 if(!Geo_SphsXaabb3(cent,rad,bpt1,bpt2)) return 0;
1956 Geo_Hemis2Rect(cent,rad,outvect,r1,r2,r3,r4);
1957 return Geo_RectXaabb3(r1,r2,r3,r4,bpt1,bpt2); }
1958
1959
Geo_CylsXaabb3(double * pt1,double * pt2,double rad,double * bpt1,double * bpt2)1960 int Geo_CylsXaabb3(double *pt1,double *pt2,double rad,double *bpt1,double *bpt2) {
1961 double r1[3],r2[3],r3[3],r4[3];
1962
1963 if(!Geo_CylisXaabb3(pt1,pt2,rad,bpt1,bpt2)) return 0;
1964 Geo_Cyl2Rect(pt1,pt2,rad,r1,r2,r3,r4);
1965 return Geo_RectXaabb3(r1,r2,r3,r4,bpt1,bpt2); }
1966
1967
1968 /******************** volumes ****************/
1969
Geo_SphVolume(double rad,int dim)1970 double Geo_SphVolume(double rad,int dim) {
1971 double vol;
1972
1973 if(dim==0) vol=1;
1974 else if(dim==1) vol=2*rad;
1975 else if(dim==2) vol=PI*rad*rad;
1976 else if(dim==3) vol=4.0/3.0*PI*rad*rad*rad;
1977 else vol=2.0/(dim*exp(gammaln(dim/2.0)))*pow(SQRTPI*rad,dim);
1978 return vol; }
1979
1980
Geo_SphOLSph(double * cent1,double * cent2,double r1,double r2,int dim)1981 double Geo_SphOLSph(double *cent1,double *cent2,double r1,double r2,int dim) {
1982 double ol,dist;
1983 int d;
1984
1985 dist=0;
1986 for(d=0;d<dim;d++) dist+=(cent2[d]-cent1[d])*(cent2[d]-cent1[d]);
1987 dist=sqrt(dist);
1988 if(dist>=r1+r2) ol=0;
1989 else if(dist+r2<=r1) ol=Geo_SphVolume(r2,dim);
1990 else if(dist+r1<=r2) ol=Geo_SphVolume(r1,dim);
1991 else {
1992 if(dim==1)
1993 ol=r1+r2-dist;
1994 else if(dim==2)
1995 ol=r1*r1*acos((dist*dist+r1*r1-r2*r2)/(2*dist*r1))+r2*r2*acos((dist*dist+r2*r2-r1*r1)/(2*dist*r2))-0.5*sqrt((-dist+r1+r2)*(dist+r1-r2)*(dist-r1+r2)*(dist+r1+r2));
1996 else if(dim==3)
1997 ol=PI*(r2+r1-dist)*(r2+r1-dist)*(dist*dist+2*dist*r1-3*r1*r1+2*dist*r2+6*r1*r2-3*r2*r2)/(12*dist);
1998 else ol=-1; }
1999 return ol; }
2000
2001
2002 /******************** testing *******************/
2003
2004 // To run test, rename below function to main and compile and link with:
2005 // gcc -Wall -O0 -g Geometry.c math2.c -o testcode
2006
2007
main_Geometry(void)2008 int main_Geometry(void) {
2009 return 0; }
2010
2011
2012
2013