== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / BulletCollision / Gimpact / gim_tri_collision.h
1 #ifndef GIM_TRI_COLLISION_H_INCLUDED
2 #define GIM_TRI_COLLISION_H_INCLUDED
3
4 /*! \file gim_tri_collision.h
5 \author Francisco Le\7fn N├čjera
6 */
7 /*
8 -----------------------------------------------------------------------------
9 This source file is part of GIMPACT Library.
10
11 For the latest info, see http://gimpact.sourceforge.net/
12
13 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
14 email: 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
45 struct 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
125 class GIM_TRIANGLE
126 {
127 public:
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
243 if 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