1 /*************************************************************************
2  *                                                                       *
3  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
4  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
5  *                                                                       *
6  * This library is free software; you can redistribute it and/or         *
7  * modify it under the terms of EITHER:                                  *
8  *   (1) The GNU Lesser General Public License as published by the Free  *
9  *       Software Foundation; either version 2.1 of the License, or (at  *
10  *       your option) any later version. The text of the GNU Lesser      *
11  *       General Public License is included with this library in the     *
12  *       file LICENSE.TXT.                                               *
13  *   (2) The BSD-style license that is included with this library in     *
14  *       the file LICENSE-BSD.TXT.                                       *
15  *                                                                       *
16  * This library is distributed in the hope that it will be useful,       *
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
19  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
20  *                                                                       *
21  *************************************************************************/
22 
23 /*
24 
25 some useful collision utility stuff. this includes some API utility
26 functions that are defined in the public header files.
27 
28 */
29 
30 #include <ode/common.h>
31 #include <ode/collision.h>
32 #include <ode/odemath.h>
33 #include "collision_util.h"
34 
35 //****************************************************************************
36 
dCollideSpheres(dVector3 p1,dReal r1,dVector3 p2,dReal r2,dContactGeom * c)37 int dCollideSpheres (dVector3 p1, dReal r1,
38 		     dVector3 p2, dReal r2, dContactGeom *c)
39 {
40   // printf ("d=%.2f  (%.2f %.2f %.2f) (%.2f %.2f %.2f) r1=%.2f r2=%.2f\n",
41   //	  d,p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],r1,r2);
42 
43   dReal d = dDISTANCE (p1,p2);
44   if (d > (r1 + r2)) return 0;
45   if (d <= 0) {
46     c->pos[0] = p1[0];
47     c->pos[1] = p1[1];
48     c->pos[2] = p1[2];
49     c->normal[0] = 1;
50     c->normal[1] = 0;
51     c->normal[2] = 0;
52     c->depth = r1 + r2;
53   }
54   else {
55     dReal d1 = dRecip (d);
56     c->normal[0] = (p1[0]-p2[0])*d1;
57     c->normal[1] = (p1[1]-p2[1])*d1;
58     c->normal[2] = (p1[2]-p2[2])*d1;
59     dReal k = REAL(0.5) * (r2 - r1 - d);
60     c->pos[0] = p1[0] + c->normal[0]*k;
61     c->pos[1] = p1[1] + c->normal[1]*k;
62     c->pos[2] = p1[2] + c->normal[2]*k;
63     c->depth = r1 + r2 - d;
64   }
65   return 1;
66 }
67 
68 
dLineClosestApproach(const dVector3 pa,const dVector3 ua,const dVector3 pb,const dVector3 ub,dReal * alpha,dReal * beta)69 void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
70 			   const dVector3 pb, const dVector3 ub,
71 			   dReal *alpha, dReal *beta)
72 {
73   dVector3 p;
74   p[0] = pb[0] - pa[0];
75   p[1] = pb[1] - pa[1];
76   p[2] = pb[2] - pa[2];
77   dReal uaub = dDOT(ua,ub);
78   dReal q1 =  dDOT(ua,p);
79   dReal q2 = -dDOT(ub,p);
80   dReal d = 1-uaub*uaub;
81   if (d <= REAL(0.0001)) {
82     // @@@ this needs to be made more robust
83     *alpha = 0;
84     *beta  = 0;
85   }
86   else {
87     d = dRecip(d);
88     *alpha = (q1 + uaub*q2)*d;
89     *beta  = (uaub*q1 + q2)*d;
90   }
91 }
92 
93 
94 // given two line segments A and B with endpoints a1-a2 and b1-b2, return the
95 // points on A and B that are closest to each other (in cp1 and cp2).
96 // in the case of parallel lines where there are multiple solutions, a
97 // solution involving the endpoint of at least one line will be returned.
98 // this will work correctly for zero length lines, e.g. if a1==a2 and/or
99 // b1==b2.
100 //
101 // the algorithm works by applying the voronoi clipping rule to the features
102 // of the line segments. the three features of each line segment are the two
103 // endpoints and the line between them. the voronoi clipping rule states that,
104 // for feature X on line A and feature Y on line B, the closest points PA and
105 // PB between X and Y are globally the closest points if PA is in V(Y) and
106 // PB is in V(X), where V(X) is the voronoi region of X.
107 
dClosestLineSegmentPoints(const dVector3 a1,const dVector3 a2,const dVector3 b1,const dVector3 b2,dVector3 cp1,dVector3 cp2)108 void dClosestLineSegmentPoints (const dVector3 a1, const dVector3 a2,
109 				const dVector3 b1, const dVector3 b2,
110 				dVector3 cp1, dVector3 cp2)
111 {
112   dVector3 a1a2,b1b2,a1b1,a1b2,a2b1,a2b2,n;
113   dReal la,lb,k,da1,da2,da3,da4,db1,db2,db3,db4,det;
114 
115 #define SET2(a,b) a[0]=b[0]; a[1]=b[1]; a[2]=b[2];
116 #define SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2];
117 
118   // check vertex-vertex features
119 
120   SET3 (a1a2,a2,-,a1);
121   SET3 (b1b2,b2,-,b1);
122   SET3 (a1b1,b1,-,a1);
123   da1 = dDOT(a1a2,a1b1);
124   db1 = dDOT(b1b2,a1b1);
125   if (da1 <= 0 && db1 >= 0) {
126     SET2 (cp1,a1);
127     SET2 (cp2,b1);
128     return;
129   }
130 
131   SET3 (a1b2,b2,-,a1);
132   da2 = dDOT(a1a2,a1b2);
133   db2 = dDOT(b1b2,a1b2);
134   if (da2 <= 0 && db2 <= 0) {
135     SET2 (cp1,a1);
136     SET2 (cp2,b2);
137     return;
138   }
139 
140   SET3 (a2b1,b1,-,a2);
141   da3 = dDOT(a1a2,a2b1);
142   db3 = dDOT(b1b2,a2b1);
143   if (da3 >= 0 && db3 >= 0) {
144     SET2 (cp1,a2);
145     SET2 (cp2,b1);
146     return;
147   }
148 
149   SET3 (a2b2,b2,-,a2);
150   da4 = dDOT(a1a2,a2b2);
151   db4 = dDOT(b1b2,a2b2);
152   if (da4 >= 0 && db4 <= 0) {
153     SET2 (cp1,a2);
154     SET2 (cp2,b2);
155     return;
156   }
157 
158   // check edge-vertex features.
159   // if one or both of the lines has zero length, we will never get to here,
160   // so we do not have to worry about the following divisions by zero.
161 
162   la = dDOT(a1a2,a1a2);
163   if (da1 >= 0 && da3 <= 0) {
164     k = da1 / la;
165     SET3 (n,a1b1,-,k*a1a2);
166     if (dDOT(b1b2,n) >= 0) {
167       SET3 (cp1,a1,+,k*a1a2);
168       SET2 (cp2,b1);
169       return;
170     }
171   }
172 
173   if (da2 >= 0 && da4 <= 0) {
174     k = da2 / la;
175     SET3 (n,a1b2,-,k*a1a2);
176     if (dDOT(b1b2,n) <= 0) {
177       SET3 (cp1,a1,+,k*a1a2);
178       SET2 (cp2,b2);
179       return;
180     }
181   }
182 
183   lb = dDOT(b1b2,b1b2);
184   if (db1 <= 0 && db2 >= 0) {
185     k = -db1 / lb;
186     SET3 (n,-a1b1,-,k*b1b2);
187     if (dDOT(a1a2,n) >= 0) {
188       SET2 (cp1,a1);
189       SET3 (cp2,b1,+,k*b1b2);
190       return;
191     }
192   }
193 
194   if (db3 <= 0 && db4 >= 0) {
195     k = -db3 / lb;
196     SET3 (n,-a2b1,-,k*b1b2);
197     if (dDOT(a1a2,n) <= 0) {
198       SET2 (cp1,a2);
199       SET3 (cp2,b1,+,k*b1b2);
200       return;
201     }
202   }
203 
204   // it must be edge-edge
205 
206   k = dDOT(a1a2,b1b2);
207   det = la*lb - k*k;
208   if (det <= 0) {
209     // this should never happen, but just in case...
210     SET2(cp1,a1);
211     SET2(cp2,b1);
212     return;
213   }
214   det = dRecip (det);
215   dReal alpha = (lb*da1 -  k*db1) * det;
216   dReal beta  = ( k*da1 - la*db1) * det;
217   SET3 (cp1,a1,+,alpha*a1a2);
218   SET3 (cp2,b1,+,beta*b1b2);
219 
220 # undef SET2
221 # undef SET3
222 }
223 
224 
225 // a simple root finding algorithm is used to find the value of 't' that
226 // satisfies:
227 //		d|D(t)|^2/dt = 0
228 // where:
229 //		|D(t)| = |p(t)-b(t)|
230 // where p(t) is a point on the line parameterized by t:
231 //		p(t) = p1 + t*(p2-p1)
232 // and b(t) is that same point clipped to the boundary of the box. in box-
233 // relative coordinates d|D(t)|^2/dt is the sum of three x,y,z components
234 // each of which looks like this:
235 //
236 //	    t_lo     /
237 //	      ______/    -->t
238 //	     /     t_hi
239 //	    /
240 //
241 // t_lo and t_hi are the t values where the line passes through the planes
242 // corresponding to the sides of the box. the algorithm computes d|D(t)|^2/dt
243 // in a piecewise fashion from t=0 to t=1, stopping at the point where
244 // d|D(t)|^2/dt crosses from negative to positive.
245 
dClosestLineBoxPoints(const dVector3 p1,const dVector3 p2,const dVector3 c,const dMatrix3 R,const dVector3 side,dVector3 lret,dVector3 bret)246 void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2,
247 			    const dVector3 c, const dMatrix3 R,
248 			    const dVector3 side,
249 			    dVector3 lret, dVector3 bret)
250 {
251   int i;
252 
253   // compute the start and delta of the line p1-p2 relative to the box.
254   // we will do all subsequent computations in this box-relative coordinate
255   // system. we have to do a translation and rotation for each point.
256   dVector3 tmp,s,v;
257   tmp[0] = p1[0] - c[0];
258   tmp[1] = p1[1] - c[1];
259   tmp[2] = p1[2] - c[2];
260   dMULTIPLY1_331 (s,R,tmp);
261   tmp[0] = p2[0] - p1[0];
262   tmp[1] = p2[1] - p1[1];
263   tmp[2] = p2[2] - p1[2];
264   dMULTIPLY1_331 (v,R,tmp);
265 
266   // mirror the line so that v has all components >= 0
267   dVector3 sign;
268   for (i=0; i<3; i++) {
269     if (v[i] < 0) {
270       s[i] = -s[i];
271       v[i] = -v[i];
272       sign[i] = -1;
273     }
274     else sign[i] = 1;
275   }
276 
277   // compute v^2
278   dVector3 v2;
279   v2[0] = v[0]*v[0];
280   v2[1] = v[1]*v[1];
281   v2[2] = v[2]*v[2];
282 
283   // compute the half-sides of the box
284   dReal h[3];
285   h[0] = REAL(0.5) * side[0];
286   h[1] = REAL(0.5) * side[1];
287   h[2] = REAL(0.5) * side[2];
288 
289   // region is -1,0,+1 depending on which side of the box planes each
290   // coordinate is on. tanchor is the next t value at which there is a
291   // transition, or the last one if there are no more.
292   int region[3];
293   dReal tanchor[3];
294 
295   // Denormals are a problem, because we divide by v[i], and then
296   // multiply that by 0. Alas, infinity times 0 is infinity (!)
297   // We also use v2[i], which is v[i] squared. Here's how the epsilons
298   // are chosen:
299   // float epsilon = 1.175494e-038 (smallest non-denormal number)
300   // double epsilon = 2.225074e-308 (smallest non-denormal number)
301   // For single precision, choose an epsilon such that v[i] squared is
302   // not a denormal; this is for performance.
303   // For double precision, choose an epsilon such that v[i] is not a
304   // denormal; this is for correctness. (Jon Watte on mailinglist)
305 
306 #if defined( dSINGLE )
307   const dReal tanchor_eps = REAL(1e-19);
308 #else
309   const dReal tanchor_eps = REAL(1e-307);
310 #endif
311 
312   // find the region and tanchor values for p1
313   for (i=0; i<3; i++) {
314     if (v[i] > tanchor_eps) {
315       if (s[i] < -h[i]) {
316 	region[i] = -1;
317 	tanchor[i] = (-h[i]-s[i])/v[i];
318       }
319       else {
320 	region[i] = (s[i] > h[i]);
321 	tanchor[i] = (h[i]-s[i])/v[i];
322       }
323     }
324     else {
325       region[i] = 0;
326       tanchor[i] = 2;		// this will never be a valid tanchor
327     }
328   }
329 
330   // compute d|d|^2/dt for t=0. if it's >= 0 then p1 is the closest point
331   dReal t=0;
332   dReal dd2dt = 0;
333   for (i=0; i<3; i++) dd2dt -= (region[i] ? v2[i] : 0) * tanchor[i];
334   if (dd2dt >= 0) goto got_answer;
335 
336   do {
337     // find the point on the line that is at the next clip plane boundary
338     dReal next_t = 1;
339     for (i=0; i<3; i++) {
340       if (tanchor[i] > t && tanchor[i] < 1 && tanchor[i] < next_t)
341         next_t = tanchor[i];
342     }
343 
344     // compute d|d|^2/dt for the next t
345     dReal next_dd2dt = 0;
346     for (i=0; i<3; i++) {
347       next_dd2dt += (region[i] ? v2[i] : 0) * (next_t - tanchor[i]);
348     }
349 
350     // if the sign of d|d|^2/dt has changed, solution = the crossover point
351     if (next_dd2dt >= 0) {
352       dReal m = (next_dd2dt-dd2dt)/(next_t - t);
353       t -= dd2dt/m;
354       goto got_answer;
355     }
356 
357     // advance to the next anchor point / region
358     for (i=0; i<3; i++) {
359       if (tanchor[i] == next_t) {
360 	tanchor[i] = (h[i]-s[i])/v[i];
361 	region[i]++;
362       }
363     }
364     t = next_t;
365     dd2dt = next_dd2dt;
366   }
367   while (t < 1);
368   t = 1;
369 
370   got_answer:
371 
372   // compute closest point on the line
373   for (i=0; i<3; i++) lret[i] = p1[i] + t*tmp[i];	// note: tmp=p2-p1
374 
375   // compute closest point on the box
376   for (i=0; i<3; i++) {
377     tmp[i] = sign[i] * (s[i] + t*v[i]);
378     if (tmp[i] < -h[i]) tmp[i] = -h[i];
379     else if (tmp[i] > h[i]) tmp[i] = h[i];
380   }
381   dMULTIPLY0_331 (s,R,tmp);
382   for (i=0; i<3; i++) bret[i] = s[i] + c[i];
383 }
384 
385 
386 // given boxes (p1,R1,side1) and (p1,R1,side1), return 1 if they intersect
387 // or 0 if not.
388 
dBoxTouchesBox(const dVector3 p1,const dMatrix3 R1,const dVector3 side1,const dVector3 p2,const dMatrix3 R2,const dVector3 side2)389 int dBoxTouchesBox (const dVector3 p1, const dMatrix3 R1,
390 		    const dVector3 side1, const dVector3 p2,
391 		    const dMatrix3 R2, const dVector3 side2)
392 {
393   // two boxes are disjoint if (and only if) there is a separating axis
394   // perpendicular to a face from one box or perpendicular to an edge from
395   // either box. the following tests are derived from:
396   //    "OBB Tree: A Hierarchical Structure for Rapid Interference Detection",
397   //    S.Gottschalk, M.C.Lin, D.Manocha., Proc of ACM Siggraph 1996.
398 
399   // Rij is R1'*R2, i.e. the relative rotation between R1 and R2.
400   // Qij is abs(Rij)
401   dVector3 p,pp;
402   dReal A1,A2,A3,B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33,
403     Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33;
404 
405   // get vector from centers of box 1 to box 2, relative to box 1
406   p[0] = p2[0] - p1[0];
407   p[1] = p2[1] - p1[1];
408   p[2] = p2[2] - p1[2];
409   dMULTIPLY1_331 (pp,R1,p);		// get pp = p relative to body 1
410 
411   // get side lengths / 2
412   A1 = side1[0]*REAL(0.5); A2 = side1[1]*REAL(0.5); A3 = side1[2]*REAL(0.5);
413   B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5);
414 
415   // for the following tests, excluding computation of Rij, in the worst case,
416   // 15 compares, 60 adds, 81 multiplies, and 24 absolutes.
417   // notation: R1=[u1 u2 u3], R2=[v1 v2 v3]
418 
419   // separating axis = u1,u2,u3
420   R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
421   Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
422   if (dFabs(pp[0]) > (A1 + B1*Q11 + B2*Q12 + B3*Q13)) return 0;
423   R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
424   Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
425   if (dFabs(pp[1]) > (A2 + B1*Q21 + B2*Q22 + B3*Q23)) return 0;
426   R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
427   Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
428   if (dFabs(pp[2]) > (A3 + B1*Q31 + B2*Q32 + B3*Q33)) return 0;
429 
430   // separating axis = v1,v2,v3
431   if (dFabs(dDOT41(R2+0,p)) > (A1*Q11 + A2*Q21 + A3*Q31 + B1)) return 0;
432   if (dFabs(dDOT41(R2+1,p)) > (A1*Q12 + A2*Q22 + A3*Q32 + B2)) return 0;
433   if (dFabs(dDOT41(R2+2,p)) > (A1*Q13 + A2*Q23 + A3*Q33 + B3)) return 0;
434 
435   // separating axis = u1 x (v1,v2,v3)
436   if (dFabs(pp[2]*R21-pp[1]*R31) > A2*Q31 + A3*Q21 + B2*Q13 + B3*Q12) return 0;
437   if (dFabs(pp[2]*R22-pp[1]*R32) > A2*Q32 + A3*Q22 + B1*Q13 + B3*Q11) return 0;
438   if (dFabs(pp[2]*R23-pp[1]*R33) > A2*Q33 + A3*Q23 + B1*Q12 + B2*Q11) return 0;
439 
440   // separating axis = u2 x (v1,v2,v3)
441   if (dFabs(pp[0]*R31-pp[2]*R11) > A1*Q31 + A3*Q11 + B2*Q23 + B3*Q22) return 0;
442   if (dFabs(pp[0]*R32-pp[2]*R12) > A1*Q32 + A3*Q12 + B1*Q23 + B3*Q21) return 0;
443   if (dFabs(pp[0]*R33-pp[2]*R13) > A1*Q33 + A3*Q13 + B1*Q22 + B2*Q21) return 0;
444 
445   // separating axis = u3 x (v1,v2,v3)
446   if (dFabs(pp[1]*R11-pp[0]*R21) > A1*Q21 + A2*Q11 + B2*Q33 + B3*Q32) return 0;
447   if (dFabs(pp[1]*R12-pp[0]*R22) > A1*Q22 + A2*Q12 + B1*Q33 + B3*Q31) return 0;
448   if (dFabs(pp[1]*R13-pp[0]*R23) > A1*Q23 + A2*Q13 + B1*Q32 + B2*Q31) return 0;
449 
450   return 1;
451 }
452 
453 //****************************************************************************
454 // other utility functions
455 
dInfiniteAABB(dxGeom * geom,dReal aabb[6])456 void dInfiniteAABB (dxGeom *geom, dReal aabb[6])
457 {
458   aabb[0] = -dInfinity;
459   aabb[1] = dInfinity;
460   aabb[2] = -dInfinity;
461   aabb[3] = dInfinity;
462   aabb[4] = -dInfinity;
463   aabb[5] = dInfinity;
464 }
465 
466 
467 //****************************************************************************
468 // Helpers for Croteam's collider - by Nguyen Binh
469 
dClipEdgeToPlane(dVector3 & vEpnt0,dVector3 & vEpnt1,const dVector4 & plPlane)470 int dClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane)
471 {
472 	// calculate distance of edge points to plane
473 	dReal fDistance0 = dPointPlaneDistance(  vEpnt0 ,plPlane );
474 	dReal fDistance1 = dPointPlaneDistance(  vEpnt1 ,plPlane );
475 
476 	// if both points are behind the plane
477 	if ( fDistance0 < 0 && fDistance1 < 0 )
478 	{
479 		// do nothing
480 		return 0;
481 		// if both points in front of the plane
482 	}
483 	else if ( fDistance0 > 0 && fDistance1 > 0 )
484 	{
485 		// accept them
486 		return 1;
487 		// if we have edge/plane intersection
488 	} else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0))
489 	{
490 
491 		// find intersection point of edge and plane
492 		dVector3 vIntersectionPoint;
493 		vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1);
494 		vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1);
495 		vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1);
496 
497 		// clamp correct edge to intersection point
498 		if ( fDistance0 < 0 )
499 		{
500 			dVector3Copy(vIntersectionPoint,vEpnt0);
501 		} else
502 		{
503 			dVector3Copy(vIntersectionPoint,vEpnt1);
504 		}
505 		return 1;
506 	}
507 	return 1;
508 }
509 
510 // clip polygon with plane and generate new polygon points
dClipPolyToPlane(const dVector3 avArrayIn[],const int ctIn,dVector3 avArrayOut[],int & ctOut,const dVector4 & plPlane)511 void		 dClipPolyToPlane( const dVector3 avArrayIn[], const int ctIn,
512 							  dVector3 avArrayOut[], int &ctOut,
513 							  const dVector4 &plPlane )
514 {
515 	// start with no output points
516 	ctOut = 0;
517 
518 	int i0 = ctIn-1;
519 
520 	// for each edge in input polygon
521 	for (int i1=0; i1<ctIn; i0=i1, i1++) {
522 
523 
524 		// calculate distance of edge points to plane
525 		dReal fDistance0 = dPointPlaneDistance(  avArrayIn[i0],plPlane );
526 		dReal fDistance1 = dPointPlaneDistance(  avArrayIn[i1],plPlane );
527 
528 		// if first point is in front of plane
529 		if( fDistance0 >= 0 ) {
530 			// emit point
531 			avArrayOut[ctOut][0] = avArrayIn[i0][0];
532 			avArrayOut[ctOut][1] = avArrayIn[i0][1];
533 			avArrayOut[ctOut][2] = avArrayIn[i0][2];
534 			ctOut++;
535 		}
536 
537 		// if points are on different sides
538 		if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) {
539 
540 			// find intersection point of edge and plane
541 			dVector3 vIntersectionPoint;
542 			vIntersectionPoint[0]= avArrayIn[i0][0] -
543 				(avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
544 			vIntersectionPoint[1]= avArrayIn[i0][1] -
545 				(avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
546 			vIntersectionPoint[2]= avArrayIn[i0][2] -
547 				(avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);
548 
549 			// emit intersection point
550 			avArrayOut[ctOut][0] = vIntersectionPoint[0];
551 			avArrayOut[ctOut][1] = vIntersectionPoint[1];
552 			avArrayOut[ctOut][2] = vIntersectionPoint[2];
553 			ctOut++;
554 		}
555 	}
556 
557 }
558 
dClipPolyToCircle(const dVector3 avArrayIn[],const int ctIn,dVector3 avArrayOut[],int & ctOut,const dVector4 & plPlane,dReal fRadius)559 void		 dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn,
560 							   dVector3 avArrayOut[], int &ctOut,
561 							   const dVector4 &plPlane ,dReal fRadius)
562 {
563 	// start with no output points
564 	ctOut = 0;
565 
566 	int i0 = ctIn-1;
567 
568 	// for each edge in input polygon
569 	for (int i1=0; i1<ctIn; i0=i1, i1++)
570 	{
571 		// calculate distance of edge points to plane
572 		dReal fDistance0 = dPointPlaneDistance(  avArrayIn[i0],plPlane );
573 		dReal fDistance1 = dPointPlaneDistance(  avArrayIn[i1],plPlane );
574 
575 		// if first point is in front of plane
576 		if( fDistance0 >= 0 )
577 		{
578 			// emit point
579 			if (dVector3Length2(avArrayIn[i0]) <= fRadius*fRadius)
580 			{
581 				avArrayOut[ctOut][0] = avArrayIn[i0][0];
582 				avArrayOut[ctOut][1] = avArrayIn[i0][1];
583 				avArrayOut[ctOut][2] = avArrayIn[i0][2];
584 				ctOut++;
585 			}
586 		}
587 
588 		// if points are on different sides
589 		if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) )
590 		{
591 
592 			// find intersection point of edge and plane
593 			dVector3 vIntersectionPoint;
594 			vIntersectionPoint[0]= avArrayIn[i0][0] -
595 				(avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
596 			vIntersectionPoint[1]= avArrayIn[i0][1] -
597 				(avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
598 			vIntersectionPoint[2]= avArrayIn[i0][2] -
599 				(avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);
600 
601 			// emit intersection point
602 			if (dVector3Length2(avArrayIn[i0]) <= fRadius*fRadius)
603 			{
604 				avArrayOut[ctOut][0] = vIntersectionPoint[0];
605 				avArrayOut[ctOut][1] = vIntersectionPoint[1];
606 				avArrayOut[ctOut][2] = vIntersectionPoint[2];
607 				ctOut++;
608 			}
609 		}
610 	}
611 }
612 
613