1 
2 /*
3 -----------------------------------------------------------------------------
4 This source file is part of GIMPACT Library.
5 
6 For the latest info, see http://gimpact.sourceforge.net/
7 
8 Copyright (c) 2006 Francisco Leon. C.C. 80087371.
9 email: projectileman@yahoo.com
10 
11  This library is free software; you can redistribute it and/or
12  modify it under the terms of EITHER:
13    (1) The GNU Lesser General Public License as published by the Free
14        Software Foundation; either version 2.1 of the License, or (at
15        your option) any later version. The text of the GNU Lesser
16        General Public License is included with this library in the
17        file GIMPACT-LICENSE-LGPL.TXT.
18    (2) The BSD-style license that is included with this library in
19        the file GIMPACT-LICENSE-BSD.TXT.
20 
21  This library is distributed in the hope that it will be useful,
22  but WITHOUT ANY WARRANTY; without even the implied warranty of
23  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
24  GIMPACT-LICENSE-LGPL.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
25 
26 -----------------------------------------------------------------------------
27 */
28 
29 #include "GIMPACT/gim_trimesh.h"
30 
31 //! Utility function for find the closest point between a segment and a triangle
32 /*!
33 
34 \param triangle
35 \param s1
36 \param s2
37 \param contacts Contains the closest points on the segment (1,2), and the normal points to segment, and m_depth contains the distance
38 
39 \post The contacts array is not set to 0. It adds aditional contacts
40 */
gim_closest_point_triangle_segment(GIM_TRIANGLE_DATA * triangle,vec3f s1,vec3f s2,GDYNAMIC_ARRAY * contacts)41 void gim_closest_point_triangle_segment(GIM_TRIANGLE_DATA * triangle, vec3f s1,vec3f s2, GDYNAMIC_ARRAY * contacts)
42 {
43     vec3f segment_points[4] = {{0}};
44     vec3f closest_points[2] = {{0}};
45     GUINT32 intersection_type, out_edge= 10;
46     GREAL dis, dis_temp,perpend;
47     vec4f sdiff;
48 
49     dis = DISTANCE_PLANE_POINT(triangle->m_planes.m_planes[0],s1);
50     dis_temp = DISTANCE_PLANE_POINT(triangle->m_planes.m_planes[0],s2);
51 
52     if(dis<=0.0f && dis_temp<=0.0f) return;
53 
54     VEC_DIFF(sdiff,s2,s1);
55     perpend = VEC_DOT(sdiff,triangle->m_planes.m_planes[0]);
56 
57     if(!IS_ZERO(perpend)) // Not perpendicular
58     {
59         if(dis<dis_temp)
60         {
61             VEC_COPY(closest_points[0],s1);
62         }
63         else
64         {
65             dis = dis_temp;
66             VEC_COPY(closest_points[0],s2);
67         }
68 
69         //Testing segment vertices over triangle
70         if(dis>=0.0f && dis_temp>=0.0f)
71         {
72             POINT_IN_HULL(closest_points[0],(&triangle->m_planes.m_planes[1]),3,out_edge);
73 
74             if(out_edge==0)//Point over face
75             {
76                 GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
77                 return;
78             }
79         }
80         else
81         {
82 
83             PLANE_CLIP_SEGMENT(s1,s2,triangle->m_planes.m_planes[0],closest_points[1]);
84 
85             POINT_IN_HULL(closest_points[1],(&triangle->m_planes.m_planes[1]),3,out_edge);
86 
87             if(out_edge==0)//Point over face
88             {
89                 GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
90                 return;
91             }
92         }
93 
94     }
95     else // Perpendicular Face
96     {
97         //out_edge=10
98         //Clip segment by triangle
99     //    Edge1
100         PLANE_CLIP_SEGMENT_CLOSEST(s1,s2,triangle->m_planes.m_planes[1],segment_points[0],segment_points[1],intersection_type);
101         if(intersection_type==0||intersection_type==1)
102         {
103             out_edge = 0;
104             VEC_COPY(closest_points[0],segment_points[0]);
105         }
106         else
107         {
108             //Edge2
109             PLANE_CLIP_SEGMENT_CLOSEST(segment_points[0],segment_points[1],triangle->m_planes.m_planes[2],segment_points[2],segment_points[3],intersection_type);
110             if(intersection_type==0||intersection_type==1)
111             {
112                 out_edge = 1;
113                 VEC_COPY(closest_points[0],segment_points[3]);
114             }
115             else
116             {
117                 //Edge3
118                 PLANE_CLIP_SEGMENT_CLOSEST(segment_points[2],segment_points[3],triangle->m_planes.m_planes[3],closest_points[0],closest_points[1],intersection_type);
119                 if(intersection_type==0||intersection_type==1)
120                 {
121                     out_edge = 2;
122                 }
123             }
124         }
125         //POST closest_points[0] and closest_points[1] are inside the triangle, if out_edge>2
126         if(out_edge>2) // Over triangle
127         {
128             dis = VEC_DOT(closest_points[0],triangle->m_planes.m_planes[0]);
129             GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
130             GIM_PUSH_CONTACT((*contacts),closest_points[1] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
131             return;
132         }
133     }
134 
135     //Find closest edges
136     out_edge = 10;
137     dis = G_REAL_INFINITY;
138     GUINT32 i;
139     for(i=0;i<3;i++)
140     {
141         SEGMENT_COLLISION(s1,s2,triangle->m_vertices[i],triangle->m_vertices[(i+1)%3],segment_points[0],segment_points[1]);
142         VEC_DIFF(sdiff,segment_points[0],segment_points[1]);
143         dis_temp = VEC_DOT(sdiff,sdiff);
144         if(dis_temp< dis)
145         {
146             dis = dis_temp;
147             out_edge = i;
148             VEC_COPY(closest_points[0],segment_points[0]);
149             VEC_COPY(closest_points[1],sdiff);//normal
150         }
151     }
152     if(out_edge>2) return ;// ???? ASSERT this please
153 
154     if(IS_ZERO(dis))
155     {
156         //Set face plane
157         GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,0.0f,0, 0, 0,0);
158 
159     }
160     else
161     {
162         GIM_SQRT(dis,dis);
163         VEC_SCALE(closest_points[1],(1.0f/dis),closest_points[1]);//normal
164         GIM_PUSH_CONTACT((*contacts),closest_points[0] ,closest_points[1],dis,0, 0, 0,0);
165     }
166 }
167 
168 
169 //! Utility function for find the closest point between a capsule and a triangle
170 /*!
171 
172 \param triangle
173 \param capsule
174 \param contacts Contains the closest points on the capsule, and the normal points to triangle
175 
176 \post The contacts array is not set to 0. It adds aditional contacts
177 */
gim_triangle_capsule_collision(GIM_TRIANGLE_DATA * triangle,GIM_CAPSULE_DATA * capsule,GDYNAMIC_ARRAY * contacts)178 int gim_triangle_capsule_collision(GIM_TRIANGLE_DATA * triangle, GIM_CAPSULE_DATA * capsule, GDYNAMIC_ARRAY * contacts)
179 {
180     GUINT32 old_contact_size = contacts->m_size;
181     gim_closest_point_triangle_segment(triangle,capsule->m_point1,capsule->m_point2,contacts);
182 
183     if (contacts->m_size == old_contact_size)
184     {
185         return 0;
186     }
187 
188 	GIM_CONTACT * pcontact = GIM_DYNARRAY_POINTER(GIM_CONTACT ,(*contacts));
189     pcontact+= old_contact_size;
190 
191     if(pcontact->m_depth > capsule->m_radius)
192     {
193         contacts->m_size = old_contact_size;
194         return 0;
195     }
196 
197     vec3f vec;
198     while(old_contact_size<contacts->m_size)
199     {
200         //Scale the normal for pointing to triangle
201         VEC_SCALE(pcontact->m_normal,-1.0f,pcontact->m_normal);
202         //Fix the contact point
203         VEC_SCALE(vec,capsule->m_radius,pcontact->m_normal);
204         VEC_SUM(pcontact->m_point,vec,pcontact->m_point);
205         //Fix the depth
206         pcontact->m_depth = capsule->m_radius - pcontact->m_depth;
207 
208         pcontact++;
209         old_contact_size++;
210     }
211 
212     return 1;
213 }
214 
215 
216 //! Trimesh Capsule collision
217 /*!
218 Find the closest primitive collided by the ray
219 \param trimesh
220 \param capsule
221 \param contact
222 \param contacts A GIM_CONTACT array. Must be initialized
223 */
gim_trimesh_capsule_collision(GIM_TRIMESH * trimesh,GIM_CAPSULE_DATA * capsule,GDYNAMIC_ARRAY * contacts)224 void gim_trimesh_capsule_collision(GIM_TRIMESH * trimesh, GIM_CAPSULE_DATA * capsule, GDYNAMIC_ARRAY * contacts)
225 {
226     contacts->m_size = 0;
227 
228     aabb3f test_aabb;
229     CALC_CAPSULE_AABB((*capsule),test_aabb);
230 
231 	GDYNAMIC_ARRAY collision_result;
232 	GIM_CREATE_BOXQUERY_LIST(collision_result);
233 
234 	gim_aabbset_box_collision(&test_aabb, &trimesh->m_aabbset , &collision_result);
235 
236 	if(collision_result.m_size==0)
237 	{
238 	    GIM_DYNARRAY_DESTROY(collision_result);
239 	}
240 
241 	//collide triangles
242 	//Locks trimesh
243 	gim_trimesh_locks_work_data(trimesh);
244 	 //dummy contacts
245     GDYNAMIC_ARRAY dummycontacts;
246     GIM_CREATE_CONTACT_LIST(dummycontacts);
247 
248 	int cresult;
249 	unsigned int i;
250 	GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result);
251 	GIM_TRIANGLE_DATA tri_data;
252 	GUINT32 old_contact_size;
253 	GIM_CONTACT * pcontact;
254 
255 	for(i=0;i<collision_result.m_size;i++)
256 	{
257 	    old_contact_size = dummycontacts.m_size;
258 		gim_trimesh_get_triangle_data(trimesh,boxesresult[i],&tri_data);
259 		cresult = gim_triangle_capsule_collision(&tri_data, capsule, &dummycontacts);
260 		if(cresult!=0)
261 		{
262 		    pcontact = GIM_DYNARRAY_POINTER(GIM_CONTACT ,dummycontacts);
263             pcontact+= old_contact_size;
264 		    while(old_contact_size<dummycontacts.m_size)
265             {
266                 pcontact->m_handle1 = trimesh;
267                 pcontact->m_handle2 = capsule;
268                 pcontact->m_feature1 = boxesresult[i];
269                 pcontact->m_feature2 = 0;
270                 pcontact++;
271                 old_contact_size++;
272             }
273 		}
274 	}
275 	///unlocks
276 	gim_trimesh_unlocks_work_data(trimesh);
277 	///Destroy box result
278 	GIM_DYNARRAY_DESTROY(collision_result);
279 
280 	 //merge contacts
281     gim_merge_contacts(&dummycontacts,contacts);
282 
283     //Destroy dummy
284     GIM_DYNARRAY_DESTROY(dummycontacts);
285 }
286