Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/Gimpact/gim_basic_geometry_operations.h @ 6727

Last change on this file since 6727 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

  • Property svn:eol-style set to native
File size: 13.2 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
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
96template<typename CLASS_POINT,typename CLASS_PLANE>
97SIMD_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
109template<typename CLASS_POINT,typename CLASS_PLANE>
110SIMD_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
122enum ePLANE_INTERSECTION_TYPE
123{
124        G_BACK_PLANE = 0,
125        G_COLLIDE_PLANE,
126        G_FRONT_PLANE
127};
128
129enum 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/*!
141intersection 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
152template<typename CLASS_POINT,typename CLASS_PLANE>
153SIMD_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/*!
181clipped1 and clipped2 are the vertices behind the plane.
182clipped1 is the closest
183
184intersection_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*/
194template<typename CLASS_POINT,typename CLASS_PLANE>
195SIMD_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/*!
236Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).<br/>
237It uses the PLANEDIREPSILON constant.
238*/
239template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
240SIMD_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*/
266template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
267SIMD_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*/
312template<typename CLASS_POINT,typename CLASS_PLANE>
313SIMD_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 */
337template<typename CLASS_POINT>
338SIMD_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*/
374template<typename T,typename CLASS_POINT>
375SIMD_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
398template<typename CLASS_POINT>
399SIMD_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*/
497template<typename T>
498SIMD_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
515template<typename T>
516SIMD_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
Note: See TracBrowser for help on using the repository browser.