aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/collision_util.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_util.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/collision_util.cpp612
1 files changed, 0 insertions, 612 deletions
diff --git a/libraries/ode-0.9/ode/src/collision_util.cpp b/libraries/ode-0.9/ode/src/collision_util.cpp
deleted file mode 100644
index 460cc20..0000000
--- a/libraries/ode-0.9/ode/src/collision_util.cpp
+++ /dev/null
@@ -1,612 +0,0 @@
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
25some useful collision utility stuff. this includes some API utility
26functions 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
37int 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
69void 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
108void 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
246void 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
389int 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
456void 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
470int 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
511void 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
559void 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
generated by cgit v1.1 at 2024-11-11 06:10:26 +0000