Bullet Collision Detection & Physics Library
gim_basic_geometry_operations.h
Go to the documentation of this file.
1#ifndef GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
2#define GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
3
9/*
10-----------------------------------------------------------------------------
11This source file is part of GIMPACT Library.
12
13For the latest info, see http://gimpact.sourceforge.net/
14
15Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
16email: projectileman@yahoo.com
17
18 This library is free software; you can redistribute it and/or
19 modify it under the terms of EITHER:
20 (1) The GNU Lesser General Public License as published by the Free
21 Software Foundation; either version 2.1 of the License, or (at
22 your option) any later version. The text of the GNU Lesser
23 General Public License is included with this library in the
24 file GIMPACT-LICENSE-LGPL.TXT.
25 (2) The BSD-style license that is included with this library in
26 the file GIMPACT-LICENSE-BSD.TXT.
27 (3) The zlib/libpng license that is included with this library in
28 the file GIMPACT-LICENSE-ZLIB.TXT.
29
30 This library is distributed in the hope that it will be useful,
31 but WITHOUT ANY WARRANTY; without even the implied warranty of
32 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
33 GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
34
35-----------------------------------------------------------------------------
36*/
37
38
39#include "gim_linear_math.h"
40
41
42
43
44#ifndef PLANEDIREPSILON
45#define PLANEDIREPSILON 0.0000001f
46#endif
47
48#ifndef PARALELENORMALS
49#define PARALELENORMALS 0.000001f
50#endif
51
52#define TRIANGLE_NORMAL(v1,v2,v3,n)\
53{\
54 vec3f _dif1,_dif2;\
55 VEC_DIFF(_dif1,v2,v1);\
56 VEC_DIFF(_dif2,v3,v1);\
57 VEC_CROSS(n,_dif1,_dif2);\
58 VEC_NORMALIZE(n);\
59}\
60
61#define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\
62 vec3f _dif1,_dif2; \
63 VEC_DIFF(_dif1,v2,v1); \
64 VEC_DIFF(_dif2,v3,v1); \
65 VEC_CROSS(n,_dif1,_dif2); \
66}\
67
69#define TRIANGLE_PLANE(v1,v2,v3,plane) {\
70 TRIANGLE_NORMAL(v1,v2,v3,plane);\
71 plane[3] = VEC_DOT(v1,plane);\
72}\
73
75#define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\
76 TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\
77 plane[3] = VEC_DOT(v1,plane);\
78}\
79
81#define EDGE_PLANE(e1,e2,n,plane) {\
82 vec3f _dif; \
83 VEC_DIFF(_dif,e2,e1); \
84 VEC_CROSS(plane,_dif,n); \
85 VEC_NORMALIZE(plane); \
86 plane[3] = VEC_DOT(e1,plane);\
87}\
88
89#define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3])
90
91#define PROJECT_POINT_PLANE(point,plane,projected) {\
92 GREAL _dis;\
93 _dis = DISTANCE_PLANE_POINT(plane,point);\
94 VEC_SCALE(projected,-_dis,plane);\
95 VEC_SUM(projected,projected,point); \
96}\
97
99template<typename CLASS_POINT,typename CLASS_PLANE>
101 const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count)
102{
103 GREAL _dis;
104 for (GUINT _i = 0;_i< plane_count;++_i)
105 {
106 _dis = DISTANCE_PLANE_POINT(planes[_i],point);
107 if(_dis>0.0f) return false;
108 }
109 return true;
110}
111
112template<typename CLASS_POINT,typename CLASS_PLANE>
114 const CLASS_POINT& s1,
115 const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped)
116{
117 GREAL _dis1,_dis2;
118 _dis1 = DISTANCE_PLANE_POINT(plane,s1);
119 VEC_DIFF(clipped,s2,s1);
120 _dis2 = VEC_DOT(clipped,plane);
121 VEC_SCALE(clipped,-_dis1/_dis2,clipped);
122 VEC_SUM(clipped,clipped,s1);
123}
124
126{
131
133{
141
143
155template<typename CLASS_POINT,typename CLASS_PLANE>
157 const CLASS_POINT& s1,
158 const CLASS_POINT &s2,
159 const CLASS_PLANE &plane,CLASS_POINT &clipped)
160{
161 GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1);
162 GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2);
163 if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON)
164 {
165 if(_dis1<_dis2) return G_FRONT_PLANE_S1;
166 return G_FRONT_PLANE_S2;
167 }
168 else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON)
169 {
170 if(_dis1>_dis2) return G_BACK_PLANE_S1;
171 return G_BACK_PLANE_S2;
172 }
173
174 VEC_DIFF(clipped,s2,s1);
175 _dis2 = VEC_DOT(clipped,plane);
176 VEC_SCALE(clipped,-_dis1/_dis2,clipped);
177 VEC_SUM(clipped,clipped,s1);
178 if(_dis1<_dis2) return G_COLLIDE_PLANE_S1;
179 return G_COLLIDE_PLANE_S2;
180}
181
183
197template<typename CLASS_POINT,typename CLASS_PLANE>
199 const CLASS_POINT& s1,
200 const CLASS_POINT &s2,
201 const CLASS_PLANE &plane,
202 CLASS_POINT &clipped1,CLASS_POINT &clipped2)
203{
204 eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1);
205 switch(intersection_type)
206 {
207 case G_FRONT_PLANE_S1:
208 VEC_COPY(clipped1,s1);
209 VEC_COPY(clipped2,s2);
210 break;
211 case G_FRONT_PLANE_S2:
212 VEC_COPY(clipped1,s2);
213 VEC_COPY(clipped2,s1);
214 break;
215 case G_BACK_PLANE_S1:
216 VEC_COPY(clipped1,s1);
217 VEC_COPY(clipped2,s2);
218 break;
219 case G_BACK_PLANE_S2:
220 VEC_COPY(clipped1,s2);
221 VEC_COPY(clipped2,s1);
222 break;
224 VEC_COPY(clipped2,s1);
225 break;
227 VEC_COPY(clipped2,s2);
228 break;
229 }
230 return intersection_type;
231}
232
233
235#define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1)
236
238
242template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
244 const CLASS_PLANE & plane,
245 const CLASS_POINT & vDir,
246 const CLASS_POINT & vPoint,
247 CLASS_POINT & pout,T &tparam)
248{
249 GREAL _dis,_dotdir;
250 _dotdir = VEC_DOT(plane,vDir);
251 if(_dotdir<PLANEDIREPSILON)
252 {
253 return false;
254 }
255 _dis = DISTANCE_PLANE_POINT(plane,vPoint);
256 tparam = -_dis/_dotdir;
257 VEC_SCALE(pout,tparam,vDir);
258 VEC_SUM(pout,vPoint,pout);
259 return true;
260}
261
263
269template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
271 const CLASS_PLANE & plane,
272 const CLASS_POINT & vDir,
273 const CLASS_POINT & vPoint,
274 CLASS_POINT & pout,
275 T &tparam,
276 T tmin, T tmax)
277{
278 GREAL _dis,_dotdir;
279 _dotdir = VEC_DOT(plane,vDir);
280 if(btFabs(_dotdir)<PLANEDIREPSILON)
281 {
282 tparam = tmax;
283 return 0;
284 }
285 _dis = DISTANCE_PLANE_POINT(plane,vPoint);
286 char returnvalue = _dis<0.0f?2:1;
287 tparam = -_dis/_dotdir;
288
289 if(tparam<tmin)
290 {
291 returnvalue = 0;
292 tparam = tmin;
293 }
294 else if(tparam>tmax)
295 {
296 returnvalue = 0;
297 tparam = tmax;
298 }
299
300 VEC_SCALE(pout,tparam,vDir);
301 VEC_SUM(pout,vPoint,pout);
302 return returnvalue;
303}
304
315template<typename CLASS_POINT,typename CLASS_PLANE>
317 const CLASS_PLANE &p1,
318 const CLASS_PLANE &p2,
319 CLASS_POINT &p,
320 CLASS_POINT &d)
321{
322 VEC_CROSS(d,p1,p2);
323 GREAL denom = VEC_DOT(d, d);
324 if(GIM_IS_ZERO(denom)) return false;
325 vec3f _n;
326 _n[0]=p1[3]*p2[0] - p2[3]*p1[0];
327 _n[1]=p1[3]*p2[1] - p2[3]*p1[1];
328 _n[2]=p1[3]*p2[2] - p2[3]*p1[2];
329 VEC_CROSS(p,_n,d);
330 p[0]/=denom;
331 p[1]/=denom;
332 p[2]/=denom;
333 return true;
334}
335
336//***************** SEGMENT and LINE FUNCTIONS **********************************///
337
340template<typename CLASS_POINT>
342 CLASS_POINT & cp, const CLASS_POINT & v,
343 const CLASS_POINT &e1,const CLASS_POINT &e2)
344{
345 vec3f _n;
346 VEC_DIFF(_n,e2,e1);
347 VEC_DIFF(cp,v,e1);
348 GREAL _scalar = VEC_DOT(cp, _n);
349 _scalar/= VEC_DOT(_n, _n);
350 if(_scalar <0.0f)
351 {
352 VEC_COPY(cp,e1);
353 }
354 else if(_scalar >1.0f)
355 {
356 VEC_COPY(cp,e2);
357 }
358 else
359 {
360 VEC_SCALE(cp,_scalar,_n);
361 VEC_SUM(cp,cp,e1);
362 }
363}
364
365
377template<typename T,typename CLASS_POINT>
379 const CLASS_POINT & dir1,
380 CLASS_POINT & point1,
381 const CLASS_POINT & dir2,
382 CLASS_POINT & point2,
383 T& t1,T& t2)
384{
385 GREAL det;
386 GREAL e1e1 = VEC_DOT(dir1,dir1);
387 GREAL e1e2 = VEC_DOT(dir1,dir2);
388 GREAL e2e2 = VEC_DOT(dir2,dir2);
389 vec3f p1p2;
390 VEC_DIFF(p1p2,point1,point2);
391 GREAL p1p2e1 = VEC_DOT(p1p2,dir1);
392 GREAL p1p2e2 = VEC_DOT(p1p2,dir2);
393 det = e1e2*e1e2 - e1e1*e2e2;
394 if(GIM_IS_ZERO(det)) return false;
395 t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det;
396 t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det;
397 return true;
398}
399
401template<typename CLASS_POINT>
403 const CLASS_POINT & vA1,
404 const CLASS_POINT & vA2,
405 const CLASS_POINT & vB1,
406 const CLASS_POINT & vB2,
407 CLASS_POINT & vPointA,
408 CLASS_POINT & vPointB)
409{
410 CLASS_POINT _AD,_BD,n;
411 vec4f _M;//plane
412 VEC_DIFF(_AD,vA2,vA1);
413 VEC_DIFF(_BD,vB2,vB1);
414 VEC_CROSS(n,_AD,_BD);
415 GREAL _tp = VEC_DOT(n,n);
416 if(_tp<G_EPSILON)//ARE PARALELE
417 {
418 //project B over A
419 bool invert_b_order = false;
420 _M[0] = VEC_DOT(vB1,_AD);
421 _M[1] = VEC_DOT(vB2,_AD);
422 if(_M[0]>_M[1])
423 {
424 invert_b_order = true;
425 GIM_SWAP_NUMBERS(_M[0],_M[1]);
426 }
427 _M[2] = VEC_DOT(vA1,_AD);
428 _M[3] = VEC_DOT(vA2,_AD);
429 //mid points
430 n[0] = (_M[0]+_M[1])*0.5f;
431 n[1] = (_M[2]+_M[3])*0.5f;
432
433 if(n[0]<n[1])
434 {
435 if(_M[1]<_M[2])
436 {
437 vPointB = invert_b_order?vB1:vB2;
438 vPointA = vA1;
439 }
440 else if(_M[1]<_M[3])
441 {
442 vPointB = invert_b_order?vB1:vB2;
443 CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
444 }
445 else
446 {
447 vPointA = vA2;
448 CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
449 }
450 }
451 else
452 {
453 if(_M[3]<_M[0])
454 {
455 vPointB = invert_b_order?vB2:vB1;
456 vPointA = vA2;
457 }
458 else if(_M[3]<_M[1])
459 {
460 vPointA = vA2;
461 CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
462 }
463 else
464 {
465 vPointB = invert_b_order?vB1:vB2;
466 CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
467 }
468 }
469 return;
470 }
471
472
473 VEC_CROSS(_M,n,_BD);
474 _M[3] = VEC_DOT(_M,vB1);
475
476 LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1));
477 /*Closest point on segment*/
478 VEC_DIFF(vPointB,vPointA,vB1);
479 _tp = VEC_DOT(vPointB, _BD);
480 _tp/= VEC_DOT(_BD, _BD);
481 _tp = GIM_CLAMP(_tp,0.0f,1.0f);
482 VEC_SCALE(vPointB,_tp,_BD);
483 VEC_SUM(vPointB,vPointB,vB1);
484}
485
486
487
488
490
500template<typename T>
501SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast)
502{
503 if(GIM_IS_ZERO(dir))
504 {
505 return !(pos < bmin || pos > bmax);
506 }
507 GREAL a0 = (bmin - pos) / dir;
508 GREAL a1 = (bmax - pos) / dir;
509 if(a0 > a1) GIM_SWAP_NUMBERS(a0, a1);
510 tfirst = GIM_MAX(a0, tfirst);
511 tlast = GIM_MIN(a1, tlast);
512 if (tlast < tfirst) return false;
513 return true;
514}
515
516
518template<typename T>
520 const T * values,
521 GUINT * order_indices)
522{
523 //get minimum
524 order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2);
525
526 //get second and third
527 GUINT i0 = (order_indices[0] + 1)%3;
528 GUINT i1 = (i0 + 1)%3;
529
530 if(values[i0] < values[i1])
531 {
532 order_indices[1] = i0;
533 order_indices[2] = i1;
534 }
535 else
536 {
537 order_indices[1] = i1;
538 order_indices[2] = i0;
539 }
540}
541
542
543
544
545
546#endif // GIM_VECTOR_H_INCLUDED
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btScalar btFabs(btScalar x)
Definition: btScalar.h:475
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
#define DISTANCE_PLANE_POINT(plane, point)
eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(const CLASS_POINT &s1, const CLASS_POINT &s2, const CLASS_PLANE &plane, CLASS_POINT &clipped1, CLASS_POINT &clipped2)
Confirms if the plane intersect the edge or not.
void SORT_3_INDICES(const T *values, GUINT *order_indices)
Sorts 3 componets.
eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2(const CLASS_POINT &s1, const CLASS_POINT &s2, const CLASS_PLANE &plane, CLASS_POINT &clipped)
Confirms if the plane intersect the edge or nor.
bool RAY_PLANE_COLLISION(const CLASS_PLANE &plane, const CLASS_POINT &vDir, const CLASS_POINT &vPoint, CLASS_POINT &pout, T &tparam)
Ray plane collision in one way.
bool LINE_INTERSECTION_PARAMS(const CLASS_POINT &dir1, CLASS_POINT &point1, const CLASS_POINT &dir2, CLASS_POINT &point2, T &t1, T &t2)
Finds the line params where these lines intersect.
#define PLANEDIREPSILON
bool BOX_AXIS_INTERSECT(T pos, T dir, T bmin, T bmax, T &tfirst, T &tlast)
Line box intersection in one dimension.
bool INTERSECT_PLANES(const CLASS_PLANE &p1, const CLASS_PLANE &p2, CLASS_POINT &p, CLASS_POINT &d)
Returns the Ray on which 2 planes intersect if they do. Written by Rodrigo Hernandez on ODE convex co...
GUINT LINE_PLANE_COLLISION(const CLASS_PLANE &plane, const CLASS_POINT &vDir, const CLASS_POINT &vPoint, CLASS_POINT &pout, T &tparam, T tmin, T tmax)
line collision
bool POINT_IN_HULL(const CLASS_POINT &point, const CLASS_PLANE *planes, GUINT plane_count)
Verifies if a point is in the plane hull.
void SEGMENT_COLLISION(const CLASS_POINT &vA1, const CLASS_POINT &vA2, const CLASS_POINT &vB1, const CLASS_POINT &vB2, CLASS_POINT &vPointA, CLASS_POINT &vPointB)
Find closest points on segments.
void CLOSEST_POINT_ON_SEGMENT(CLASS_POINT &cp, const CLASS_POINT &v, const CLASS_POINT &e1, const CLASS_POINT &e2)
void PLANE_CLIP_SEGMENT(const CLASS_POINT &s1, const CLASS_POINT &s2, const CLASS_PLANE &plane, CLASS_POINT &clipped)
GREAL vec4f[4]
Float vector 4D.
GREAL vec3f[3]
Float vector 3D.
#define VEC_CROSS(c, a, b)
Vector cross.
#define VEC_DOT(a, b)
Vector dot product.
#define VEC_SCALE(c, a, b)
scalar times vector
#define VEC_SUM(v21, v2, v1)
Vector sum.
#define VEC_COPY(b, a)
Copy 3D vector.
#define VEC_DIFF(v21, v2, v1)
Vector difference.
#define GIM_MAX(a, b)
Definition: gim_math.h:93
#define GREAL
Definition: gim_math.h:39
#define GIM_IS_ZERO(value)
Definition: gim_math.h:99
#define GIM_SWAP_NUMBERS(a, b)
Swap numbers.
Definition: gim_math.h:113
#define GUINT
Definition: gim_math.h:42
#define GIM_MIN(a, b)
Definition: gim_math.h:94
#define GIM_CLAMP(number, minval, maxval)
returns a clamped number
Definition: gim_math.h:108
#define G_EPSILON
Definition: gim_math.h:60