1 #ifndef GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
2 #define GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
3
4 /*! \file gim_basic_geometry_operations.h
5 *\author Francisco Leon Najera
6 type independant geometry routines
7
8 */
9 /*
10 -----------------------------------------------------------------------------
11 This source file is part of GIMPACT Library.
12
13 For the latest info, see http://gimpact.sourceforge.net/
14
15 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
16 email: 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
45 #define PLANEDIREPSILON 0.0000001f
46 #define PARALELENORMALS 0.000001f
47
48
49 #define TRIANGLE_NORMAL(v1,v2,v3,n)\
50 {\
51 vec3f _dif1,_dif2;\
52 VEC_DIFF(_dif1,v2,v1);\
53 VEC_DIFF(_dif2,v3,v1);\
54 VEC_CROSS(n,_dif1,_dif2);\
55 VEC_NORMALIZE(n);\
56 }\
57
58 #define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\
59 vec3f _dif1,_dif2; \
60 VEC_DIFF(_dif1,v2,v1); \
61 VEC_DIFF(_dif2,v3,v1); \
62 VEC_CROSS(n,_dif1,_dif2); \
63 }\
64
65 /// plane is a vec4f
66 #define TRIANGLE_PLANE(v1,v2,v3,plane) {\
67 TRIANGLE_NORMAL(v1,v2,v3,plane);\
68 plane[3] = VEC_DOT(v1,plane);\
69 }\
70
71 /// plane is a vec4f
72 #define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\
73 TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\
74 plane[3] = VEC_DOT(v1,plane);\
75 }\
76
77 /// Calc a plane from an edge an a normal. plane is a vec4f
78 #define EDGE_PLANE(e1,e2,n,plane) {\
79 vec3f _dif; \
80 VEC_DIFF(_dif,e2,e1); \
81 VEC_CROSS(plane,_dif,n); \
82 VEC_NORMALIZE(plane); \
83 plane[3] = VEC_DOT(e1,plane);\
84 }\
85
86 #define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3])
87
88 #define PROJECT_POINT_PLANE(point,plane,projected) {\
89 GREAL _dis;\
90 _dis = DISTANCE_PLANE_POINT(plane,point);\
91 VEC_SCALE(projected,-_dis,plane);\
92 VEC_SUM(projected,projected,point); \
93 }\
94
95 //! Verifies if a point is in the plane hull
96 template<typename CLASS_POINT,typename CLASS_PLANE>
POINT_IN_HULL(const CLASS_POINT & point,const CLASS_PLANE * planes,GUINT plane_count)97 SIMD_FORCE_INLINE bool POINT_IN_HULL(
98 const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count)
99 {
100 GREAL _dis;
101 for (GUINT _i = 0;_i< plane_count;++_i)
102 {
103 _dis = DISTANCE_PLANE_POINT(planes[_i],point);
104 if(_dis>0.0f) return false;
105 }
106 return true;
107 }
108
109 template<typename CLASS_POINT,typename CLASS_PLANE>
PLANE_CLIP_SEGMENT(const CLASS_POINT & s1,const CLASS_POINT & s2,const CLASS_PLANE & plane,CLASS_POINT & clipped)110 SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT(
111 const CLASS_POINT& s1,
112 const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped)
113 {
114 GREAL _dis1,_dis2;
115 _dis1 = DISTANCE_PLANE_POINT(plane,s1);
116 VEC_DIFF(clipped,s2,s1);
117 _dis2 = VEC_DOT(clipped,plane);
118 VEC_SCALE(clipped,-_dis1/_dis2,clipped);
119 VEC_SUM(clipped,clipped,s1);
120 }
121
122 enum ePLANE_INTERSECTION_TYPE
123 {
124 G_BACK_PLANE = 0,
125 G_COLLIDE_PLANE,
126 G_FRONT_PLANE
127 };
128
129 enum eLINE_PLANE_INTERSECTION_TYPE
130 {
131 G_FRONT_PLANE_S1 = 0,
132 G_FRONT_PLANE_S2,
133 G_BACK_PLANE_S1,
134 G_BACK_PLANE_S2,
135 G_COLLIDE_PLANE_S1,
136 G_COLLIDE_PLANE_S2
137 };
138
139 //! Confirms if the plane intersect the edge or nor
140 /*!
141 intersection type must have the following values
142 <ul>
143 <li> 0 : Segment in front of plane, s1 closest
144 <li> 1 : Segment in front of plane, s2 closest
145 <li> 2 : Segment in back of plane, s1 closest
146 <li> 3 : Segment in back of plane, s2 closest
147 <li> 4 : Segment collides plane, s1 in back
148 <li> 5 : Segment collides plane, s2 in back
149 </ul>
150 */
151
152 template<typename CLASS_POINT,typename CLASS_PLANE>
PLANE_CLIP_SEGMENT2(const CLASS_POINT & s1,const CLASS_POINT & s2,const CLASS_PLANE & plane,CLASS_POINT & clipped)153 SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2(
154 const CLASS_POINT& s1,
155 const CLASS_POINT &s2,
156 const CLASS_PLANE &plane,CLASS_POINT &clipped)
157 {
158 GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1);
159 GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2);
160 if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON)
161 {
162 if(_dis1<_dis2) return G_FRONT_PLANE_S1;
163 return G_FRONT_PLANE_S2;
164 }
165 else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON)
166 {
167 if(_dis1>_dis2) return G_BACK_PLANE_S1;
168 return G_BACK_PLANE_S2;
169 }
170
171 VEC_DIFF(clipped,s2,s1);
172 _dis2 = VEC_DOT(clipped,plane);
173 VEC_SCALE(clipped,-_dis1/_dis2,clipped);
174 VEC_SUM(clipped,clipped,s1);
175 if(_dis1<_dis2) return G_COLLIDE_PLANE_S1;
176 return G_COLLIDE_PLANE_S2;
177 }
178
179 //! Confirms if the plane intersect the edge or not
180 /*!
181 clipped1 and clipped2 are the vertices behind the plane.
182 clipped1 is the closest
183
184 intersection_type must have the following values
185 <ul>
186 <li> 0 : Segment in front of plane, s1 closest
187 <li> 1 : Segment in front of plane, s2 closest
188 <li> 2 : Segment in back of plane, s1 closest
189 <li> 3 : Segment in back of plane, s2 closest
190 <li> 4 : Segment collides plane, s1 in back
191 <li> 5 : Segment collides plane, s2 in back
192 </ul>
193 */
194 template<typename CLASS_POINT,typename CLASS_PLANE>
PLANE_CLIP_SEGMENT_CLOSEST(const CLASS_POINT & s1,const CLASS_POINT & s2,const CLASS_PLANE & plane,CLASS_POINT & clipped1,CLASS_POINT & clipped2)195 SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(
196 const CLASS_POINT& s1,
197 const CLASS_POINT &s2,
198 const CLASS_PLANE &plane,
199 CLASS_POINT &clipped1,CLASS_POINT &clipped2)
200 {
201 eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1);
202 switch(intersection_type)
203 {
204 case G_FRONT_PLANE_S1:
205 VEC_COPY(clipped1,s1);
206 VEC_COPY(clipped2,s2);
207 break;
208 case G_FRONT_PLANE_S2:
209 VEC_COPY(clipped1,s2);
210 VEC_COPY(clipped2,s1);
211 break;
212 case G_BACK_PLANE_S1:
213 VEC_COPY(clipped1,s1);
214 VEC_COPY(clipped2,s2);
215 break;
216 case G_BACK_PLANE_S2:
217 VEC_COPY(clipped1,s2);
218 VEC_COPY(clipped2,s1);
219 break;
220 case G_COLLIDE_PLANE_S1:
221 VEC_COPY(clipped2,s1);
222 break;
223 case G_COLLIDE_PLANE_S2:
224 VEC_COPY(clipped2,s2);
225 break;
226 }
227 return intersection_type;
228 }
229
230
231 //! Finds the 2 smallest cartesian coordinates of a plane normal
232 #define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1)
233
234 //! Ray plane collision in one way
235 /*!
236 Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).<br/>
237 It uses the PLANEDIREPSILON constant.
238 */
239 template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
RAY_PLANE_COLLISION(const CLASS_PLANE & plane,const CLASS_POINT & vDir,const CLASS_POINT & vPoint,CLASS_POINT & pout,T & tparam)240 SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION(
241 const CLASS_PLANE & plane,
242 const CLASS_POINT & vDir,
243 const CLASS_POINT & vPoint,
244 CLASS_POINT & pout,T &tparam)
245 {
246 GREAL _dis,_dotdir;
247 _dotdir = VEC_DOT(plane,vDir);
248 if(_dotdir<PLANEDIREPSILON)
249 {
250 return false;
251 }
252 _dis = DISTANCE_PLANE_POINT(plane,vPoint);
253 tparam = -_dis/_dotdir;
254 VEC_SCALE(pout,tparam,vDir);
255 VEC_SUM(pout,vPoint,pout);
256 return true;
257 }
258
259 //! line collision
260 /*!
261 *\return
262 -0 if the ray never intersects
263 -1 if the ray collides in front
264 -2 if the ray collides in back
265 */
266 template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
LINE_PLANE_COLLISION(const CLASS_PLANE & plane,const CLASS_POINT & vDir,const CLASS_POINT & vPoint,CLASS_POINT & pout,T & tparam,T tmin,T tmax)267 SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION(
268 const CLASS_PLANE & plane,
269 const CLASS_POINT & vDir,
270 const CLASS_POINT & vPoint,
271 CLASS_POINT & pout,
272 T &tparam,
273 T tmin, T tmax)
274 {
275 GREAL _dis,_dotdir;
276 _dotdir = VEC_DOT(plane,vDir);
277 if(btFabs(_dotdir)<PLANEDIREPSILON)
278 {
279 tparam = tmax;
280 return 0;
281 }
282 _dis = DISTANCE_PLANE_POINT(plane,vPoint);
283 char returnvalue = _dis<0.0f?2:1;
284 tparam = -_dis/_dotdir;
285
286 if(tparam<tmin)
287 {
288 returnvalue = 0;
289 tparam = tmin;
290 }
291 else if(tparam>tmax)
292 {
293 returnvalue = 0;
294 tparam = tmax;
295 }
296
297 VEC_SCALE(pout,tparam,vDir);
298 VEC_SUM(pout,vPoint,pout);
299 return returnvalue;
300 }
301
302 /*! \brief Returns the Ray on which 2 planes intersect if they do.
303 Written by Rodrigo Hernandez on ODE convex collision
304
305 \param p1 Plane 1
306 \param p2 Plane 2
307 \param p Contains the origin of the ray upon returning if planes intersect
308 \param d Contains the direction of the ray upon returning if planes intersect
309 \return true if the planes intersect, 0 if paralell.
310
311 */
312 template<typename CLASS_POINT,typename CLASS_PLANE>
INTERSECT_PLANES(const CLASS_PLANE & p1,const CLASS_PLANE & p2,CLASS_POINT & p,CLASS_POINT & d)313 SIMD_FORCE_INLINE bool INTERSECT_PLANES(
314 const CLASS_PLANE &p1,
315 const CLASS_PLANE &p2,
316 CLASS_POINT &p,
317 CLASS_POINT &d)
318 {
319 VEC_CROSS(d,p1,p2);
320 GREAL denom = VEC_DOT(d, d);
321 if(GIM_IS_ZERO(denom)) return false;
322 vec3f _n;
323 _n[0]=p1[3]*p2[0] - p2[3]*p1[0];
324 _n[1]=p1[3]*p2[1] - p2[3]*p1[1];
325 _n[2]=p1[3]*p2[2] - p2[3]*p1[2];
326 VEC_CROSS(p,_n,d);
327 p[0]/=denom;
328 p[1]/=denom;
329 p[2]/=denom;
330 return true;
331 }
332
333 //***************** SEGMENT and LINE FUNCTIONS **********************************///
334
335 /*! Finds the closest point(cp) to (v) on a segment (e1,e2)
336 */
337 template<typename CLASS_POINT>
CLOSEST_POINT_ON_SEGMENT(CLASS_POINT & cp,const CLASS_POINT & v,const CLASS_POINT & e1,const CLASS_POINT & e2)338 SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT(
339 CLASS_POINT & cp, const CLASS_POINT & v,
340 const CLASS_POINT &e1,const CLASS_POINT &e2)
341 {
342 vec3f _n;
343 VEC_DIFF(_n,e2,e1);
344 VEC_DIFF(cp,v,e1);
345 GREAL _scalar = VEC_DOT(cp, _n);
346 _scalar/= VEC_DOT(_n, _n);
347 if(_scalar <0.0f)
348 {
349 VEC_COPY(cp,e1);
350 }
351 else if(_scalar >1.0f)
352 {
353 VEC_COPY(cp,e2);
354 }
355 else
356 {
357 VEC_SCALE(cp,_scalar,_n);
358 VEC_SUM(cp,cp,e1);
359 }
360 }
361
362
363 /*! \brief Finds the line params where these lines intersect.
364
365 \param dir1 Direction of line 1
366 \param point1 Point of line 1
367 \param dir2 Direction of line 2
368 \param point2 Point of line 2
369 \param t1 Result Parameter for line 1
370 \param t2 Result Parameter for line 2
371 \param dointersect 0 if the lines won't intersect, else 1
372
373 */
374 template<typename T,typename CLASS_POINT>
LINE_INTERSECTION_PARAMS(const CLASS_POINT & dir1,CLASS_POINT & point1,const CLASS_POINT & dir2,CLASS_POINT & point2,T & t1,T & t2)375 SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS(
376 const CLASS_POINT & dir1,
377 CLASS_POINT & point1,
378 const CLASS_POINT & dir2,
379 CLASS_POINT & point2,
380 T& t1,T& t2)
381 {
382 GREAL det;
383 GREAL e1e1 = VEC_DOT(dir1,dir1);
384 GREAL e1e2 = VEC_DOT(dir1,dir2);
385 GREAL e2e2 = VEC_DOT(dir2,dir2);
386 vec3f p1p2;
387 VEC_DIFF(p1p2,point1,point2);
388 GREAL p1p2e1 = VEC_DOT(p1p2,dir1);
389 GREAL p1p2e2 = VEC_DOT(p1p2,dir2);
390 det = e1e2*e1e2 - e1e1*e2e2;
391 if(GIM_IS_ZERO(det)) return false;
392 t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det;
393 t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det;
394 return true;
395 }
396
397 //! Find closest points on segments
398 template<typename CLASS_POINT>
SEGMENT_COLLISION(const CLASS_POINT & vA1,const CLASS_POINT & vA2,const CLASS_POINT & vB1,const CLASS_POINT & vB2,CLASS_POINT & vPointA,CLASS_POINT & vPointB)399 SIMD_FORCE_INLINE void SEGMENT_COLLISION(
400 const CLASS_POINT & vA1,
401 const CLASS_POINT & vA2,
402 const CLASS_POINT & vB1,
403 const CLASS_POINT & vB2,
404 CLASS_POINT & vPointA,
405 CLASS_POINT & vPointB)
406 {
407 CLASS_POINT _AD,_BD,n;
408 vec4f _M;//plane
409 VEC_DIFF(_AD,vA2,vA1);
410 VEC_DIFF(_BD,vB2,vB1);
411 VEC_CROSS(n,_AD,_BD);
412 GREAL _tp = VEC_DOT(n,n);
413 if(_tp<G_EPSILON)//ARE PARALELE
414 {
415 //project B over A
416 bool invert_b_order = false;
417 _M[0] = VEC_DOT(vB1,_AD);
418 _M[1] = VEC_DOT(vB2,_AD);
419 if(_M[0]>_M[1])
420 {
421 invert_b_order = true;
422 GIM_SWAP_NUMBERS(_M[0],_M[1]);
423 }
424 _M[2] = VEC_DOT(vA1,_AD);
425 _M[3] = VEC_DOT(vA2,_AD);
426 //mid points
427 n[0] = (_M[0]+_M[1])*0.5f;
428 n[1] = (_M[2]+_M[3])*0.5f;
429
430 if(n[0]<n[1])
431 {
432 if(_M[1]<_M[2])
433 {
434 vPointB = invert_b_order?vB1:vB2;
435 vPointA = vA1;
436 }
437 else if(_M[1]<_M[3])
438 {
439 vPointB = invert_b_order?vB1:vB2;
440 CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
441 }
442 else
443 {
444 vPointA = vA2;
445 CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
446 }
447 }
448 else
449 {
450 if(_M[3]<_M[0])
451 {
452 vPointB = invert_b_order?vB2:vB1;
453 vPointA = vA2;
454 }
455 else if(_M[3]<_M[1])
456 {
457 vPointA = vA2;
458 CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
459 }
460 else
461 {
462 vPointB = invert_b_order?vB1:vB2;
463 CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
464 }
465 }
466 return;
467 }
468
469
470 VEC_CROSS(_M,n,_BD);
471 _M[3] = VEC_DOT(_M,vB1);
472
473 LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1));
474 /*Closest point on segment*/
475 VEC_DIFF(vPointB,vPointA,vB1);
476 _tp = VEC_DOT(vPointB, _BD);
477 _tp/= VEC_DOT(_BD, _BD);
478 _tp = GIM_CLAMP(_tp,0.0f,1.0f);
479 VEC_SCALE(vPointB,_tp,_BD);
480 VEC_SUM(vPointB,vPointB,vB1);
481 }
482
483
484
485
486 //! Line box intersection in one dimension
487 /*!
488
489 *\param pos Position of the ray
490 *\param dir Projection of the Direction of the ray
491 *\param bmin Minimum bound of the box
492 *\param bmax Maximum bound of the box
493 *\param tfirst the minimum projection. Assign to 0 at first.
494 *\param tlast the maximum projection. Assign to INFINITY at first.
495 *\return true if there is an intersection.
496 */
497 template<typename T>
BOX_AXIS_INTERSECT(T pos,T dir,T bmin,T bmax,T & tfirst,T & tlast)498 SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast)
499 {
500 if(GIM_IS_ZERO(dir))
501 {
502 return !(pos < bmin || pos > bmax);
503 }
504 GREAL a0 = (bmin - pos) / dir;
505 GREAL a1 = (bmax - pos) / dir;
506 if(a0 > a1) GIM_SWAP_NUMBERS(a0, a1);
507 tfirst = GIM_MAX(a0, tfirst);
508 tlast = GIM_MIN(a1, tlast);
509 if (tlast < tfirst) return false;
510 return true;
511 }
512
513
514 //! Sorts 3 componets
515 template<typename T>
SORT_3_INDICES(const T * values,GUINT * order_indices)516 SIMD_FORCE_INLINE void SORT_3_INDICES(
517 const T * values,
518 GUINT * order_indices)
519 {
520 //get minimum
521 order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2);
522
523 //get second and third
524 GUINT i0 = (order_indices[0] + 1)%3;
525 GUINT i1 = (i0 + 1)%3;
526
527 if(values[i0] < values[i1])
528 {
529 order_indices[1] = i0;
530 order_indices[2] = i1;
531 }
532 else
533 {
534 order_indices[1] = i1;
535 order_indices[2] = i0;
536 }
537 }
538
539
540
541
542
543 #endif // GIM_VECTOR_H_INCLUDED
544