Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/environment/src/external/bullet/BulletCollision/Gimpact/gim_tri_collision.h @ 8206

Last change on this file since 8206 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: 10.4 KB
Line 
1#ifndef GIM_TRI_COLLISION_H_INCLUDED
2#define GIM_TRI_COLLISION_H_INCLUDED
3
4/*! \file gim_tri_collision.h
5\author Francisco Len Nßjera
6*/
7/*
8-----------------------------------------------------------------------------
9This source file is part of GIMPACT Library.
10
11For the latest info, see http://gimpact.sourceforge.net/
12
13Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
14email: projectileman@yahoo.com
15
16 This library is free software; you can redistribute it and/or
17 modify it under the terms of EITHER:
18   (1) The GNU Lesser General Public License as published by the Free
19       Software Foundation; either version 2.1 of the License, or (at
20       your option) any later version. The text of the GNU Lesser
21       General Public License is included with this library in the
22       file GIMPACT-LICENSE-LGPL.TXT.
23   (2) The BSD-style license that is included with this library in
24       the file GIMPACT-LICENSE-BSD.TXT.
25   (3) The zlib/libpng license that is included with this library in
26       the file GIMPACT-LICENSE-ZLIB.TXT.
27
28 This library is distributed in the hope that it will be useful,
29 but WITHOUT ANY WARRANTY; without even the implied warranty of
30 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
31 GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
32
33-----------------------------------------------------------------------------
34*/
35
36#include "gim_box_collision.h"
37#include "gim_clip_polygon.h"
38
39
40
41
42#define MAX_TRI_CLIPPING 16
43
44//! Structure for collision
45struct GIM_TRIANGLE_CONTACT_DATA
46{
47    GREAL m_penetration_depth;
48    GUINT m_point_count;
49    btVector4 m_separating_normal;
50    btVector3 m_points[MAX_TRI_CLIPPING];
51
52        SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA& other)
53        {
54                m_penetration_depth = other.m_penetration_depth;
55                m_separating_normal = other.m_separating_normal;
56                m_point_count = other.m_point_count;
57                GUINT i = m_point_count;
58                while(i--)
59                {
60                        m_points[i] = other.m_points[i];
61                }
62        }
63
64        GIM_TRIANGLE_CONTACT_DATA()
65        {
66        }
67
68        GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA& other)
69        {
70                copy_from(other);
71        }
72
73       
74       
75
76    //! classify points that are closer
77    template<typename DISTANCE_FUNC,typename CLASS_PLANE>
78    SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE & plane,
79                                GREAL margin, const btVector3 * points, GUINT point_count, DISTANCE_FUNC distance_func)
80    {   
81        m_point_count = 0;
82        m_penetration_depth= -1000.0f;
83
84                GUINT point_indices[MAX_TRI_CLIPPING];
85
86                GUINT _k;
87
88                for(_k=0;_k<point_count;_k++)
89                {
90                        GREAL _dist = -distance_func(plane,points[_k]) + margin;
91
92                        if(_dist>=0.0f)
93                        {
94                                if(_dist>m_penetration_depth)
95                                {
96                                        m_penetration_depth = _dist;
97                                        point_indices[0] = _k;
98                                        m_point_count=1;
99                                }
100                                else if((_dist+G_EPSILON)>=m_penetration_depth)
101                                {
102                                        point_indices[m_point_count] = _k;
103                                        m_point_count++;
104                                }
105                        }
106                }
107
108                for( _k=0;_k<m_point_count;_k++)
109                {
110                        m_points[_k] = points[point_indices[_k]];
111                }
112        }
113
114        //! classify points that are closer
115        SIMD_FORCE_INLINE void merge_points(const btVector4 & plane, GREAL margin,
116                                                                                 const btVector3 * points, GUINT point_count)
117        {
118                m_separating_normal = plane;
119                mergepoints_generic(plane, margin, points, point_count, DISTANCE_PLANE_3D_FUNC());
120        }
121};
122
123
124//! Class for colliding triangles
125class GIM_TRIANGLE
126{
127public:
128        btScalar m_margin;
129    btVector3 m_vertices[3];
130
131    GIM_TRIANGLE():m_margin(0.1f)
132    {
133    }
134
135    SIMD_FORCE_INLINE GIM_AABB get_box()  const
136    {
137        return GIM_AABB(m_vertices[0],m_vertices[1],m_vertices[2],m_margin);
138    }
139
140    SIMD_FORCE_INLINE void get_normal(btVector3 &normal)  const
141    {
142        TRIANGLE_NORMAL(m_vertices[0],m_vertices[1],m_vertices[2],normal);
143    }
144
145    SIMD_FORCE_INLINE void get_plane(btVector4 &plane)  const
146    {
147        TRIANGLE_PLANE(m_vertices[0],m_vertices[1],m_vertices[2],plane);;
148    }
149
150    SIMD_FORCE_INLINE void apply_transform(const btTransform & trans)
151    {
152        m_vertices[0] = trans(m_vertices[0]);
153        m_vertices[1] = trans(m_vertices[1]);
154        m_vertices[2] = trans(m_vertices[2]);
155    }
156
157    SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index,const btVector3 &triangle_normal,btVector4 &plane)  const
158    {
159                const btVector3 & e0 = m_vertices[edge_index];
160                const btVector3 & e1 = m_vertices[(edge_index+1)%3];
161                EDGE_PLANE(e0,e1,triangle_normal,plane);
162    }
163
164    //! Gets the relative transformation of this triangle
165    /*!
166    The transformation is oriented to the triangle normal , and aligned to the 1st edge of this triangle. The position corresponds to vertice 0:
167    - triangle normal corresponds to Z axis.
168    - 1st normalized edge corresponds to X axis,
169
170    */
171    SIMD_FORCE_INLINE void get_triangle_transform(btTransform & triangle_transform)  const
172    {
173        btMatrix3x3 & matrix = triangle_transform.getBasis();
174
175        btVector3 zaxis;
176        get_normal(zaxis);
177        MAT_SET_Z(matrix,zaxis);
178
179        btVector3 xaxis = m_vertices[1] - m_vertices[0];
180        VEC_NORMALIZE(xaxis);
181        MAT_SET_X(matrix,xaxis);
182
183        //y axis
184        xaxis = zaxis.cross(xaxis);
185        MAT_SET_Y(matrix,xaxis);
186
187        triangle_transform.setOrigin(m_vertices[0]);
188    }
189
190
191        //! Test triangles by finding separating axis
192        /*!
193        \param other Triangle for collide
194        \param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle
195        */
196        bool collide_triangle_hard_test(
197                const GIM_TRIANGLE & other,
198                GIM_TRIANGLE_CONTACT_DATA & contact_data) const;
199
200        //! Test boxes before doing hard test
201        /*!
202        \param other Triangle for collide
203        \param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle
204        \
205        */
206        SIMD_FORCE_INLINE bool collide_triangle(
207                const GIM_TRIANGLE & other,
208                GIM_TRIANGLE_CONTACT_DATA & contact_data) const
209        {
210                //test box collisioin
211                GIM_AABB boxu(m_vertices[0],m_vertices[1],m_vertices[2],m_margin);
212                GIM_AABB boxv(other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin);
213                if(!boxu.has_collision(boxv)) return false;
214
215                //do hard test
216                return collide_triangle_hard_test(other,contact_data);
217        }
218
219        /*!
220
221        Solve the System for u,v parameters:
222
223        u*axe1[i1] + v*axe2[i1] = vecproj[i1]
224        u*axe1[i2] + v*axe2[i2] = vecproj[i2]
225
226        sustitute:
227        v = (vecproj[i2] - u*axe1[i2])/axe2[i2]
228
229        then the first equation in terms of 'u':
230
231        --> u*axe1[i1] + ((vecproj[i2] - u*axe1[i2])/axe2[i2])*axe2[i1] = vecproj[i1]
232
233        --> u*axe1[i1] + vecproj[i2]*axe2[i1]/axe2[i2] - u*axe1[i2]*axe2[i1]/axe2[i2] = vecproj[i1]
234
235        --> u*(axe1[i1]  - axe1[i2]*axe2[i1]/axe2[i2]) = vecproj[i1] - vecproj[i2]*axe2[i1]/axe2[i2]
236
237        --> u*((axe1[i1]*axe2[i2]  - axe1[i2]*axe2[i1])/axe2[i2]) = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1])/axe2[i2]
238
239        --> u*(axe1[i1]*axe2[i2]  - axe1[i2]*axe2[i1]) = vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]
240
241        --> u = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]) /(axe1[i1]*axe2[i2]  - axe1[i2]*axe2[i1])
242
243if 0.0<= u+v <=1.0 then they are inside of triangle
244
245        \return false if the point is outside of triangle.This function  doesn't take the margin
246        */
247        SIMD_FORCE_INLINE bool get_uv_parameters(
248                        const btVector3 & point,
249                        const btVector3 & tri_plane,
250                        GREAL & u, GREAL & v) const
251        {
252                btVector3 _axe1 = m_vertices[1]-m_vertices[0];
253                btVector3 _axe2 = m_vertices[2]-m_vertices[0];
254                btVector3 _vecproj = point - m_vertices[0];
255                GUINT _i1 = (tri_plane.closestAxis()+1)%3;
256                GUINT _i2 = (_i1+1)%3;
257                if(btFabs(_axe2[_i2])<G_EPSILON)
258                {
259                        u = (_vecproj[_i2]*_axe2[_i1] - _vecproj[_i1]*_axe2[_i2]) /(_axe1[_i2]*_axe2[_i1]  - _axe1[_i1]*_axe2[_i2]);
260                        v = (_vecproj[_i1] - u*_axe1[_i1])/_axe2[_i1];
261                }
262                else
263                {
264                        u = (_vecproj[_i1]*_axe2[_i2] - _vecproj[_i2]*_axe2[_i1]) /(_axe1[_i1]*_axe2[_i2]  - _axe1[_i2]*_axe2[_i1]);
265                        v = (_vecproj[_i2] - u*_axe1[_i2])/_axe2[_i2];
266                }
267
268                if(u<-G_EPSILON)
269                {
270                        return false;
271                }
272                else if(v<-G_EPSILON)
273                {
274                        return false;
275                }
276                else
277                {
278                        btScalar sumuv;
279                        sumuv = u+v;
280                        if(sumuv<-G_EPSILON)
281                        {
282                                return false;
283                        }
284                        else if(sumuv-1.0f>G_EPSILON)
285                        {
286                                return false;
287                        }
288                }
289                return true;
290        }
291
292        //! is point in triangle beam?
293        /*!
294        Test if point is in triangle, with m_margin tolerance
295        */
296        SIMD_FORCE_INLINE bool is_point_inside(const btVector3 & point, const btVector3 & tri_normal) const
297        {
298                //Test with edge 0
299                btVector4 edge_plane;
300                this->get_edge_plane(0,tri_normal,edge_plane);
301                GREAL dist = DISTANCE_PLANE_POINT(edge_plane,point);
302                if(dist-m_margin>0.0f) return false; // outside plane
303
304                this->get_edge_plane(1,tri_normal,edge_plane);
305                dist = DISTANCE_PLANE_POINT(edge_plane,point);
306                if(dist-m_margin>0.0f) return false; // outside plane
307
308                this->get_edge_plane(2,tri_normal,edge_plane);
309                dist = DISTANCE_PLANE_POINT(edge_plane,point);
310                if(dist-m_margin>0.0f) return false; // outside plane
311                return true;
312        }
313
314
315        //! Bidireccional ray collision
316        SIMD_FORCE_INLINE bool ray_collision(
317                const btVector3 & vPoint,
318                const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal,
319                GREAL & tparam, GREAL tmax = G_REAL_INFINITY)
320        {
321                btVector4 faceplane;
322                {
323                        btVector3 dif1 = m_vertices[1] - m_vertices[0];
324                        btVector3 dif2 = m_vertices[2] - m_vertices[0];
325                VEC_CROSS(faceplane,dif1,dif2);
326                faceplane[3] = m_vertices[0].dot(faceplane);
327                }
328
329                GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax);
330                if(res == 0) return false;
331                if(! is_point_inside(pout,faceplane)) return false;
332
333                if(res==2) //invert normal
334                {
335                        triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]);
336                }
337                else
338                {
339                        triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]);
340                }
341
342                VEC_NORMALIZE(triangle_normal);
343
344                return true;
345        }
346
347
348        //! one direccion ray collision
349        SIMD_FORCE_INLINE bool ray_collision_front_side(
350                const btVector3 & vPoint,
351                const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal,
352                GREAL & tparam, GREAL tmax = G_REAL_INFINITY)
353        {
354                btVector4 faceplane;
355                {
356                        btVector3 dif1 = m_vertices[1] - m_vertices[0];
357                        btVector3 dif2 = m_vertices[2] - m_vertices[0];
358                VEC_CROSS(faceplane,dif1,dif2);
359                faceplane[3] = m_vertices[0].dot(faceplane);
360                }
361
362                GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax);
363                if(res != 1) return false;
364
365                if(!is_point_inside(pout,faceplane)) return false;
366
367                triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]);
368
369                VEC_NORMALIZE(triangle_normal);
370
371                return true;
372        }
373
374};
375
376
377
378
379#endif // GIM_TRI_COLLISION_H_INCLUDED
Note: See TracBrowser for help on using the repository browser.