== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / BulletCollision / Gimpact / gim_basic_geometry_operations.h
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 Le\7fn N├čjera
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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