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 #include "gim_linear_math.h"
39
40 #ifndef PLANEDIREPSILON
41 #define PLANEDIREPSILON 0.0000001f
42 #endif
43
44 #ifndef PARALELENORMALS
45 #define PARALELENORMALS 0.000001f
46 #endif
47
48 #define TRIANGLE_NORMAL(v1, v2, v3, n) \
49 { \
50 vec3f _dif1, _dif2; \
51 VEC_DIFF(_dif1, v2, v1); \
52 VEC_DIFF(_dif2, v3, v1); \
53 VEC_CROSS(n, _dif1, _dif2); \
54 VEC_NORMALIZE(n); \
55 }
56
57 #define TRIANGLE_NORMAL_FAST(v1, v2, v3, n) \
58 { \
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 { \
68 TRIANGLE_NORMAL(v1, v2, v3, plane); \
69 plane[3] = VEC_DOT(v1, plane); \
70 }
71
72 /// plane is a vec4f
73 #define TRIANGLE_PLANE_FAST(v1, v2, v3, plane) \
74 { \
75 TRIANGLE_NORMAL_FAST(v1, v2, v3, plane); \
76 plane[3] = VEC_DOT(v1, plane); \
77 }
78
79 /// Calc a plane from an edge an a normal. plane is a vec4f
80 #define EDGE_PLANE(e1, e2, n, plane) \
81 { \
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 { \
93 GREAL _dis; \
94 _dis = DISTANCE_PLANE_POINT(plane, point); \
95 VEC_SCALE(projected, -_dis, plane); \
96 VEC_SUM(projected, projected, point); \
97 }
98
99 //! Verifies if a point is in the plane hull
100 template <typename CLASS_POINT, typename CLASS_PLANE>
POINT_IN_HULL(const CLASS_POINT & point,const CLASS_PLANE * planes,GUINT plane_count)101 SIMD_FORCE_INLINE bool POINT_IN_HULL(
102 const CLASS_POINT &point, const CLASS_PLANE *planes, GUINT plane_count)
103 {
104 GREAL _dis;
105 for (GUINT _i = 0; _i < plane_count; ++_i)
106 {
107 _dis = DISTANCE_PLANE_POINT(planes[_i], point);
108 if (_dis > 0.0f) return false;
109 }
110 return true;
111 }
112
113 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)114 SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT(
115 const CLASS_POINT &s1,
116 const CLASS_POINT &s2, const CLASS_PLANE &plane, CLASS_POINT &clipped)
117 {
118 GREAL _dis1, _dis2;
119 _dis1 = DISTANCE_PLANE_POINT(plane, s1);
120 VEC_DIFF(clipped, s2, s1);
121 _dis2 = VEC_DOT(clipped, plane);
122 VEC_SCALE(clipped, -_dis1 / _dis2, clipped);
123 VEC_SUM(clipped, clipped, s1);
124 }
125
126 enum ePLANE_INTERSECTION_TYPE
127 {
128 G_BACK_PLANE = 0,
129 G_COLLIDE_PLANE,
130 G_FRONT_PLANE
131 };
132
133 enum eLINE_PLANE_INTERSECTION_TYPE
134 {
135 G_FRONT_PLANE_S1 = 0,
136 G_FRONT_PLANE_S2,
137 G_BACK_PLANE_S1,
138 G_BACK_PLANE_S2,
139 G_COLLIDE_PLANE_S1,
140 G_COLLIDE_PLANE_S2
141 };
142
143 //! Confirms if the plane intersect the edge or nor
144 /*!
145 intersection type must have the following values
146 <ul>
147 <li> 0 : Segment in front of plane, s1 closest
148 <li> 1 : Segment in front of plane, s2 closest
149 <li> 2 : Segment in back of plane, s1 closest
150 <li> 3 : Segment in back of plane, s2 closest
151 <li> 4 : Segment collides plane, s1 in back
152 <li> 5 : Segment collides plane, s2 in back
153 </ul>
154 */
155
156 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)157 SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2(
158 const CLASS_POINT &s1,
159 const CLASS_POINT &s2,
160 const CLASS_PLANE &plane, CLASS_POINT &clipped)
161 {
162 GREAL _dis1 = DISTANCE_PLANE_POINT(plane, s1);
163 GREAL _dis2 = DISTANCE_PLANE_POINT(plane, s2);
164 if (_dis1 > -G_EPSILON && _dis2 > -G_EPSILON)
165 {
166 if (_dis1 < _dis2) return G_FRONT_PLANE_S1;
167 return G_FRONT_PLANE_S2;
168 }
169 else if (_dis1 < G_EPSILON && _dis2 < G_EPSILON)
170 {
171 if (_dis1 > _dis2) return G_BACK_PLANE_S1;
172 return G_BACK_PLANE_S2;
173 }
174
175 VEC_DIFF(clipped, s2, s1);
176 _dis2 = VEC_DOT(clipped, plane);
177 VEC_SCALE(clipped, -_dis1 / _dis2, clipped);
178 VEC_SUM(clipped, clipped, s1);
179 if (_dis1 < _dis2) return G_COLLIDE_PLANE_S1;
180 return G_COLLIDE_PLANE_S2;
181 }
182
183 //! Confirms if the plane intersect the edge or not
184 /*!
185 clipped1 and clipped2 are the vertices behind the plane.
186 clipped1 is the closest
187
188 intersection_type must have the following values
189 <ul>
190 <li> 0 : Segment in front of plane, s1 closest
191 <li> 1 : Segment in front of plane, s2 closest
192 <li> 2 : Segment in back of plane, s1 closest
193 <li> 3 : Segment in back of plane, s2 closest
194 <li> 4 : Segment collides plane, s1 in back
195 <li> 5 : Segment collides plane, s2 in back
196 </ul>
197 */
198 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)199 SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(
200 const CLASS_POINT &s1,
201 const CLASS_POINT &s2,
202 const CLASS_PLANE &plane,
203 CLASS_POINT &clipped1, CLASS_POINT &clipped2)
204 {
205 eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1, s2, plane, clipped1);
206 switch (intersection_type)
207 {
208 case G_FRONT_PLANE_S1:
209 VEC_COPY(clipped1, s1);
210 VEC_COPY(clipped2, s2);
211 break;
212 case G_FRONT_PLANE_S2:
213 VEC_COPY(clipped1, s2);
214 VEC_COPY(clipped2, s1);
215 break;
216 case G_BACK_PLANE_S1:
217 VEC_COPY(clipped1, s1);
218 VEC_COPY(clipped2, s2);
219 break;
220 case G_BACK_PLANE_S2:
221 VEC_COPY(clipped1, s2);
222 VEC_COPY(clipped2, s1);
223 break;
224 case G_COLLIDE_PLANE_S1:
225 VEC_COPY(clipped2, s1);
226 break;
227 case G_COLLIDE_PLANE_S2:
228 VEC_COPY(clipped2, s2);
229 break;
230 }
231 return intersection_type;
232 }
233
234 //! Finds the 2 smallest cartesian coordinates of a plane normal
235 #define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1)
236
237 //! Ray plane collision in one way
238 /*!
239 Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).<br/>
240 It uses the PLANEDIREPSILON constant.
241 */
242 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)243 SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION(
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
262 //! line collision
263 /*!
264 *\return
265 -0 if the ray never intersects
266 -1 if the ray collides in front
267 -2 if the ray collides in back
268 */
269 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)270 SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION(
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
305 /*! \brief Returns the Ray on which 2 planes intersect if they do.
306 Written by Rodrigo Hernandez on ODE convex collision
307
308 \param p1 Plane 1
309 \param p2 Plane 2
310 \param p Contains the origin of the ray upon returning if planes intersect
311 \param d Contains the direction of the ray upon returning if planes intersect
312 \return true if the planes intersect, 0 if paralell.
313
314 */
315 template <typename CLASS_POINT, typename CLASS_PLANE>
INTERSECT_PLANES(const CLASS_PLANE & p1,const CLASS_PLANE & p2,CLASS_POINT & p,CLASS_POINT & d)316 SIMD_FORCE_INLINE bool INTERSECT_PLANES(
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
338 /*! Finds the closest point(cp) to (v) on a segment (e1,e2)
339 */
340 template <typename CLASS_POINT>
CLOSEST_POINT_ON_SEGMENT(CLASS_POINT & cp,const CLASS_POINT & v,const CLASS_POINT & e1,const CLASS_POINT & e2)341 SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT(
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 /*! \brief Finds the line params where these lines intersect.
366
367 \param dir1 Direction of line 1
368 \param point1 Point of line 1
369 \param dir2 Direction of line 2
370 \param point2 Point of line 2
371 \param t1 Result Parameter for line 1
372 \param t2 Result Parameter for line 2
373 \param dointersect 0 if the lines won't intersect, else 1
374
375 */
376 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)377 SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS(
378 const CLASS_POINT &dir1,
379 CLASS_POINT &point1,
380 const CLASS_POINT &dir2,
381 CLASS_POINT &point2,
382 T &t1, T &t2)
383 {
384 GREAL det;
385 GREAL e1e1 = VEC_DOT(dir1, dir1);
386 GREAL e1e2 = VEC_DOT(dir1, dir2);
387 GREAL e2e2 = VEC_DOT(dir2, dir2);
388 vec3f p1p2;
389 VEC_DIFF(p1p2, point1, point2);
390 GREAL p1p2e1 = VEC_DOT(p1p2, dir1);
391 GREAL p1p2e2 = VEC_DOT(p1p2, dir2);
392 det = e1e2 * e1e2 - e1e1 * e2e2;
393 if (GIM_IS_ZERO(det)) return false;
394 t1 = (e1e2 * p1p2e2 - e2e2 * p1p2e1) / det;
395 t2 = (e1e1 * p1p2e2 - e1e2 * p1p2e1) / det;
396 return true;
397 }
398
399 //! Find closest points on segments
400 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)401 SIMD_FORCE_INLINE void SEGMENT_COLLISION(
402 const CLASS_POINT &vA1,
403 const CLASS_POINT &vA2,
404 const CLASS_POINT &vB1,
405 const CLASS_POINT &vB2,
406 CLASS_POINT &vPointA,
407 CLASS_POINT &vPointB)
408 {
409 CLASS_POINT _AD, _BD, n;
410 vec4f _M; //plane
411 VEC_DIFF(_AD, vA2, vA1);
412 VEC_DIFF(_BD, vB2, vB1);
413 VEC_CROSS(n, _AD, _BD);
414 GREAL _tp = VEC_DOT(n, n);
415 if (_tp < G_EPSILON) //ARE PARALELE
416 {
417 //project B over A
418 bool invert_b_order = false;
419 _M[0] = VEC_DOT(vB1, _AD);
420 _M[1] = VEC_DOT(vB2, _AD);
421 if (_M[0] > _M[1])
422 {
423 invert_b_order = true;
424 GIM_SWAP_NUMBERS(_M[0], _M[1]);
425 }
426 _M[2] = VEC_DOT(vA1, _AD);
427 _M[3] = VEC_DOT(vA2, _AD);
428 //mid points
429 n[0] = (_M[0] + _M[1]) * 0.5f;
430 n[1] = (_M[2] + _M[3]) * 0.5f;
431
432 if (n[0] < n[1])
433 {
434 if (_M[1] < _M[2])
435 {
436 vPointB = invert_b_order ? vB1 : vB2;
437 vPointA = vA1;
438 }
439 else if (_M[1] < _M[3])
440 {
441 vPointB = invert_b_order ? vB1 : vB2;
442 CLOSEST_POINT_ON_SEGMENT(vPointA, vPointB, vA1, vA2);
443 }
444 else
445 {
446 vPointA = vA2;
447 CLOSEST_POINT_ON_SEGMENT(vPointB, vPointA, vB1, vB2);
448 }
449 }
450 else
451 {
452 if (_M[3] < _M[0])
453 {
454 vPointB = invert_b_order ? vB2 : vB1;
455 vPointA = vA2;
456 }
457 else if (_M[3] < _M[1])
458 {
459 vPointA = vA2;
460 CLOSEST_POINT_ON_SEGMENT(vPointB, vPointA, vB1, vB2);
461 }
462 else
463 {
464 vPointB = invert_b_order ? vB1 : vB2;
465 CLOSEST_POINT_ON_SEGMENT(vPointA, vPointB, vA1, vA2);
466 }
467 }
468 return;
469 }
470
471 VEC_CROSS(_M, n, _BD);
472 _M[3] = VEC_DOT(_M, vB1);
473
474 LINE_PLANE_COLLISION(_M, _AD, vA1, vPointA, _tp, btScalar(0), btScalar(1));
475 /*Closest point on segment*/
476 VEC_DIFF(vPointB, vPointA, vB1);
477 _tp = VEC_DOT(vPointB, _BD);
478 _tp /= VEC_DOT(_BD, _BD);
479 _tp = GIM_CLAMP(_tp, 0.0f, 1.0f);
480 VEC_SCALE(vPointB, _tp, _BD);
481 VEC_SUM(vPointB, vPointB, vB1);
482 }
483
484 //! Line box intersection in one dimension
485 /*!
486
487 *\param pos Position of the ray
488 *\param dir Projection of the Direction of the ray
489 *\param bmin Minimum bound of the box
490 *\param bmax Maximum bound of the box
491 *\param tfirst the minimum projection. Assign to 0 at first.
492 *\param tlast the maximum projection. Assign to INFINITY at first.
493 *\return true if there is an intersection.
494 */
495 template <typename T>
BOX_AXIS_INTERSECT(T pos,T dir,T bmin,T bmax,T & tfirst,T & tlast)496 SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir, T bmin, T bmax, T &tfirst, T &tlast)
497 {
498 if (GIM_IS_ZERO(dir))
499 {
500 return !(pos < bmin || pos > bmax);
501 }
502 GREAL a0 = (bmin - pos) / dir;
503 GREAL a1 = (bmax - pos) / dir;
504 if (a0 > a1) GIM_SWAP_NUMBERS(a0, a1);
505 tfirst = GIM_MAX(a0, tfirst);
506 tlast = GIM_MIN(a1, tlast);
507 if (tlast < tfirst) return false;
508 return true;
509 }
510
511 //! Sorts 3 componets
512 template <typename T>
SORT_3_INDICES(const T * values,GUINT * order_indices)513 SIMD_FORCE_INLINE void SORT_3_INDICES(
514 const T *values,
515 GUINT *order_indices)
516 {
517 //get minimum
518 order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2);
519
520 //get second and third
521 GUINT i0 = (order_indices[0] + 1) % 3;
522 GUINT i1 = (i0 + 1) % 3;
523
524 if (values[i0] < values[i1])
525 {
526 order_indices[1] = i0;
527 order_indices[2] = i1;
528 }
529 else
530 {
531 order_indices[1] = i1;
532 order_indices[2] = i0;
533 }
534 }
535
536 #endif // GIM_VECTOR_H_INCLUDED
537