Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/GIMPACT/gim_basic_geometry_operations.h @ 2379

Last change on this file since 2379 was 2192, checked in by rgrieder, 16 years ago

Reverted all changes of attempt to update physics branch.

  • Property svn:eol-style set to native
File size: 13.3 KB
Line 
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 Len Nßjera
6type independant geometry routines
7
8*/
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/*! \defgroup GEOMETRIC_OPERATIONS
44*/
45//! @{
46
47
48#define PLANEDIREPSILON 0.0000001f
49#define PARALELENORMALS 0.000001f
50
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
68/// plane is a vec4f
69#define TRIANGLE_PLANE(v1,v2,v3,plane) {\
70    TRIANGLE_NORMAL(v1,v2,v3,plane);\
71    plane[3] = VEC_DOT(v1,plane);\
72}\
73
74/// plane is a vec4f
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
80/// Calc a plane from an edge an a normal. plane is a vec4f
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
98//! Verifies if a point is in the plane hull
99template<typename CLASS_POINT,typename CLASS_PLANE>
100SIMD_FORCE_INLINE bool POINT_IN_HULL(
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>
113SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT(
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
125enum ePLANE_INTERSECTION_TYPE
126{
127        G_BACK_PLANE = 0,
128        G_COLLIDE_PLANE,
129        G_FRONT_PLANE
130};
131
132enum eLINE_PLANE_INTERSECTION_TYPE
133{
134        G_FRONT_PLANE_S1 = 0,
135        G_FRONT_PLANE_S2,
136        G_BACK_PLANE_S1,
137        G_BACK_PLANE_S2,
138        G_COLLIDE_PLANE_S1,
139        G_COLLIDE_PLANE_S2
140};
141
142//! Confirms if the plane intersect the edge or nor
143/*!
144intersection type must have the following values
145<ul>
146<li> 0 : Segment in front of plane, s1 closest
147<li> 1 : Segment in front of plane, s2 closest
148<li> 2 : Segment in back of plane, s1 closest
149<li> 3 : Segment in back of plane, s2 closest
150<li> 4 : Segment collides plane, s1 in back
151<li> 5 : Segment collides plane, s2 in back
152</ul>
153*/
154
155template<typename CLASS_POINT,typename CLASS_PLANE>
156SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2(
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
182//! Confirms if the plane intersect the edge or not
183/*!
184clipped1 and clipped2 are the vertices behind the plane.
185clipped1 is the closest
186
187intersection_type must have the following values
188<ul>
189<li> 0 : Segment in front of plane, s1 closest
190<li> 1 : Segment in front of plane, s2 closest
191<li> 2 : Segment in back of plane, s1 closest
192<li> 3 : Segment in back of plane, s2 closest
193<li> 4 : Segment collides plane, s1 in back
194<li> 5 : Segment collides plane, s2 in back
195</ul>
196*/
197template<typename CLASS_POINT,typename CLASS_PLANE>
198SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(
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;
223        case G_COLLIDE_PLANE_S1:
224                VEC_COPY(clipped2,s1);
225                break;
226        case G_COLLIDE_PLANE_S2:
227                VEC_COPY(clipped2,s2);
228                break;
229        }
230        return intersection_type;
231}
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/*!
239Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).<br/>
240It uses the PLANEDIREPSILON constant.
241*/
242template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
243SIMD_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*/
269template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
270SIMD_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*/
315template<typename CLASS_POINT,typename CLASS_PLANE>
316SIMD_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 */
340template<typename CLASS_POINT>
341SIMD_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
366/*! \brief Finds the line params where these lines intersect.
367
368\param dir1 Direction of line 1
369\param point1 Point of line 1
370\param dir2 Direction of line 2
371\param point2 Point of line 2
372\param t1 Result Parameter for line 1
373\param t2 Result Parameter for line 2
374\param dointersect  0  if the lines won't intersect, else 1
375
376*/
377template<typename T,typename CLASS_POINT>
378SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS(
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
400//! Find closest points on segments
401template<typename CLASS_POINT>
402SIMD_FORCE_INLINE void SEGMENT_COLLISION(
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
489//! Line box intersection in one dimension
490/*!
491
492*\param pos Position of the ray
493*\param dir Projection of the Direction of the ray
494*\param bmin Minimum bound of the box
495*\param bmax Maximum bound of the box
496*\param tfirst the minimum projection. Assign to 0 at first.
497*\param tlast the maximum projection. Assign to INFINITY at first.
498*\return true if there is an intersection.
499*/
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
517//! Sorts 3 componets
518template<typename T>
519SIMD_FORCE_INLINE void SORT_3_INDICES(
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
547#endif // GIM_VECTOR_H_INCLUDED
Note: See TracBrowser for help on using the repository browser.