• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 #ifndef GIM_TRI_COLLISION_H_INCLUDED
2 #define GIM_TRI_COLLISION_H_INCLUDED
3 
4 /*! \file gim_tri_collision.h
5 \author Francisco Leon Najera
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 
copy_fromGIM_TRIANGLE_CONTACT_DATA52 	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 
GIM_TRIANGLE_CONTACT_DATAGIM_TRIANGLE_CONTACT_DATA64 	GIM_TRIANGLE_CONTACT_DATA()
65 	{
66 	}
67 
GIM_TRIANGLE_CONTACT_DATAGIM_TRIANGLE_CONTACT_DATA68 	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>
mergepoints_genericGIM_TRIANGLE_CONTACT_DATA78     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
merge_pointsGIM_TRIANGLE_CONTACT_DATA115 	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 
GIM_TRIANGLE()131     GIM_TRIANGLE():m_margin(0.1f)
132     {
133     }
134 
get_box()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 
get_normal(btVector3 & normal)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 
get_plane(btVector4 & plane)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 
apply_transform(const btTransform & trans)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 
get_edge_plane(GUINT edge_index,const btVector3 & triangle_normal,btVector4 & plane)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     */
get_triangle_transform(btTransform & triangle_transform)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 	*/
collide_triangle(const GIM_TRIANGLE & other,GIM_TRIANGLE_CONTACT_DATA & contact_data)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 	*/
get_uv_parameters(const btVector3 & point,const btVector3 & tri_plane,GREAL & u,GREAL & v)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 	*/
is_point_inside(const btVector3 & point,const btVector3 & tri_normal)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
380